hoverbrett/controller.ino

324 lines
9.0 KiB
Arduino
Raw Normal View History

//https://github.com/rogerclarkmelbourne/Arduino_STM32 in arduino/hardware
//Board: Generic STM32F103C series
//Upload method: serial
//20k RAM 64k Flash
// RX ist A10, TX ist A9 (3v3 level)
//to flash set boot0 (the one further away from reset button) to 1 and press reset, flash, program executes immediately
//set boot0 back to 0 to run program on powerup
2019-03-17 17:57:32 +00:00
//#define DEBUG
2019-03-20 22:29:09 +00:00
uint8_t error=0;
#define IMU_NO_CHANGE 2 //IMU values did not change for too long
uint8_t imu_no_change_counter=0;
#define PIN_LED PC13
2019-03-17 17:57:32 +00:00
#define SENDPERIOD 20 //ms
2019-03-20 22:29:09 +00:00
#define CONTROLUPDATEPERIOD 10
long last_controlupdate=0;
2019-03-17 17:57:32 +00:00
#define IMUUPDATEPERIOD 10 //ms
long last_imuupdated=0;
2019-03-20 22:29:09 +00:00
#define MAX_YAWCHANGE 90 //in degrees, if exceeded in one update intervall error will be triggered
2019-03-17 17:57:32 +00:00
#include <IMUGY85.h>
//https://github.com/fookingthg/GY85
//ITG3200 and ADXL345 from https://github.com/jrowberg/i2cdevlib/tree/master/Arduino
//https://github.com/mechasolution/Mecha_QMC5883L //because QMC5883 on GY85 instead of HMC5883, source: https://circuitdigest.com/microcontroller-projects/digital-compass-with-arduino-and-hmc5883l-magnetometer
//in qmc5883L library read values changed from uint16_t to int16_t
IMUGY85 imu;
2019-03-20 22:29:09 +00:00
double ax, ay, az, gx, gy, gz, roll, pitch, yaw,mx,my,mz,ma;
double old_ax, old_ay, old_az, old_gx, old_gy, old_gz, old_roll, old_pitch, old_yaw,old_mx,old_my,old_mz,old_ma;
2019-03-17 17:57:32 +00:00
double setYaw=0;
2019-03-20 22:29:09 +00:00
float magalign_multiplier=0; //how much the magnetometer should influence steering, 0=none, 1=stay aligned
2019-03-16 19:02:27 +00:00
//from left to right. pins at bottom. chips on top
//1 GND (black)
//2 Data
//3 Clock
//4 Reset
//5 +5V (red)
//6 Right BTN
//7 Middle BTN
//8 Left BTN
//pinout: https://martin-prochnow.de/projects/thinkpad_keyboard
//see also https://github.com/feklee/usb-trackpoint/blob/master/code/code.ino
2019-03-16 19:02:27 +00:00
#include <SPI.h>
#include "nRF24L01.h"
#include "RF24.h"
RF24 radio(PB0,PB1); //ce, cs
//SCK D13 (Pro mini), A5 (bluepill)
//Miso D12 (Pro mini), A6 (bluepill)
//Mosi D11 (Pro mini), A7 (bluepill)
// Radio pipe addresses for the 2 nodes to communicate.
const uint64_t pipes[2] = { 0xF0F0F0F0E1LL, 0xF0F0F0F0D2LL };
2019-03-20 22:29:09 +00:00
#define NRF24CHANNEL 75
2019-03-16 19:02:27 +00:00
struct nrfdata {
uint8_t steer;
uint8_t speed;
uint8_t commands; //bit 0 set = motor enable
uint8_t checksum;
};
2019-03-16 19:02:27 +00:00
nrfdata lastnrfdata;
long last_nrfreceive=0; //last time values were received and checksum ok
long nrf_delay=0;
#define MAX_NRFDELAY 50
2019-03-16 19:02:27 +00:00
//command variables
boolean motorenabled=false; //set by nrfdata.commands
2019-03-16 19:02:27 +00:00
long last_send=0;
int16_t out_steer=0; //between -1000 and 1000
int16_t out_speed=0;
uint8_t out_checksum=0; //0= disable motors, 255=reserved, 1<=checksum<255
#define NRFDATA_CENTER 127
2019-03-16 19:02:27 +00:00
boolean armed=false;
void setup() {
Serial.begin(115200); //Debug and Program. A9=TX1, A10=RX1 (3v3 level)
Serial2.begin(19200); //control. B10=TX3, B11=RX3 (Serial2 is Usart 3)
pinMode(PIN_LED, OUTPUT);
2019-03-16 19:02:27 +00:00
digitalWrite(PIN_LED,HIGH);
2019-03-16 19:02:27 +00:00
Serial.println("Initializing nrf24");
radio.begin();
2019-03-17 19:09:57 +00:00
radio.setDataRate( RF24_250KBPS ); //set to slow data rate. default was 1MBPS
2019-03-20 22:29:09 +00:00
radio.setChannel(NRF24CHANNEL); //0 to 124 (inclusive)
2019-03-16 19:02:27 +00:00
radio.setRetries(15,15); // optionally, increase the delay between retries & # of retries
radio.setPayloadSize(8); // optionally, reduce the payload size. seems to improve reliability
radio.openWritingPipe(pipes[0]); //write on pipe 0
radio.openReadingPipe(1,pipes[1]); //read on pipe 1
radio.startListening();
2019-03-17 17:57:32 +00:00
Serial.println("Initializing IMU");
imu.init();
2019-03-16 19:02:27 +00:00
Serial.println("Initialized");
}
void loop() {
2019-03-17 17:57:32 +00:00
if (millis()-last_imuupdated>IMUUPDATEPERIOD){
updateIMU();
last_imuupdated=millis();
}
2019-03-16 19:02:27 +00:00
//NRF24
nrf_delay=millis()-last_nrfreceive; //update nrf delay
if ( radio.available() )
{
//Serial.println("radio available ...");
bool done = false;
while (!done)
{
digitalWrite(PIN_LED, !digitalRead(PIN_LED));
done = radio.read( &lastnrfdata, sizeof(nrfdata) );
2019-03-20 22:29:09 +00:00
if (lastnrfdata.speed==NRFDATA_CENTER && lastnrfdata.speed==NRFDATA_CENTER){ //arm only when centered
armed=true; //arm at first received packet
}
2019-03-16 19:02:27 +00:00
//parse commands
motorenabled = (lastnrfdata.commands & (1 << 0)); //check bit 0
if (!motorenabled){ //disable motors?
armed=false;
}
2019-03-20 22:29:09 +00:00
2019-03-16 19:02:27 +00:00
uint8_t calcchecksum=(uint8_t)((lastnrfdata.steer+3)*(lastnrfdata.speed+13));
if (lastnrfdata.checksum!=calcchecksum){ //checksum not ok?
armed=false;
}else{ //checksum ok
last_nrfreceive=millis();
}
#ifdef DEBUG
Serial.print("Received:");
Serial.print(" st=");
Serial.print(lastnrfdata.steer);
Serial.print(", sp=");
Serial.print(lastnrfdata.speed);
Serial.print(", c=");
Serial.print(lastnrfdata.commands);
Serial.print(", chks=");
Serial.print(lastnrfdata.checksum);
Serial.print("nrfdelay=");
Serial.print(nrf_delay);
#endif
//y positive = forward
//x positive = right
2019-03-17 17:57:32 +00:00
2019-03-17 19:09:57 +00:00
/*
2019-03-17 17:57:32 +00:00
setYaw+=((int16_t)(lastnrfdata.steer)-NRFDATA_CENTER)*10/127;
while (setYaw<0){
setYaw+=360;
}
while (setYaw>=360){
setYaw-=360;
2019-03-17 19:09:57 +00:00
}*/
2019-03-17 17:57:32 +00:00
/*
Serial.print("setYaw=");
Serial.print(setYaw);
Serial.print(" Yaw=");
Serial.println(yaw);*/
2019-03-16 19:02:27 +00:00
}
}
2019-03-20 22:29:09 +00:00
if (error>0){ //disarm if error occured
armed=false;
}
2019-03-16 19:02:27 +00:00
if (armed && nrf_delay>=MAX_NRFDELAY){ //too long since last sucessful nrf receive
armed=false;
}
if (armed){
2019-03-20 22:29:09 +00:00
if (millis()-last_controlupdate>CONTROLUPDATEPERIOD){
last_controlupdate=millis();
//out_speed=(int16_t)( (lastnrfdata.y-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX );
//out_steer=(int16_t)( -(lastnrfdata.x-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX );
out_speed=(int16_t)( ((int16_t)(lastnrfdata.speed)-NRFDATA_CENTER)*1000/127 ); //-1000 to 1000
out_steer=(int16_t)( ((int16_t)(lastnrfdata.steer)-NRFDATA_CENTER)*1000/127 );
//align to compass
double yawdiff=(setYaw-180)-(yaw-180); //following angle difference works only for angles [-180,180]. yaw here is [0,360]
yawdiff += (yawdiff>180) ? -360 : (yawdiff<-180) ? 360 : 0;
//yawdiff/=2;
int yawdiffsign=1;
if (yawdiff<0){
yawdiffsign=-1;
}
yawdiff=yawdiff*yawdiff; //square
yawdiff=constrain(yawdiff*1 ,0,800);
yawdiff*=yawdiffsign; //redo sign
int16_t out_steer_mag=(int16_t)( yawdiff );
float new_magalign_multiplier=map( abs((int16_t)(lastnrfdata.steer)-NRFDATA_CENTER), 2, 10, 1.0, 0.0); //0=normal steering, 1=only mag steering
new_magalign_multiplier=constrain(new_magalign_multiplier, 0.0,1.0);
magalign_multiplier=min(new_magalign_multiplier, min(1.0,magalign_multiplier+0.01)); //go down fast, slowly increase
magalign_multiplier=constrain(magalign_multiplier, 0.0,1.0); //safety constrain again
out_steer = out_steer*(1-magalign_multiplier) + out_steer_mag*magalign_multiplier;
setYaw=setYaw*magalign_multiplier + yaw*(1-magalign_multiplier); //if magalign_multiplier 0, setYaw equals current yaw
/*
Serial.print("Out steer=");
Serial.println(out_steer);*/
2019-03-17 17:57:32 +00:00
}
2019-03-16 19:02:27 +00:00
}else{ //took too long since last nrf data
out_steer=0;
out_speed=0;
2019-03-20 22:29:09 +00:00
setYaw=yaw;
magalign_multiplier=0;
}
if (millis()-last_send>SENDPERIOD){
2019-03-16 19:02:27 +00:00
//calculate checksum
out_checksum = ((uint8_t) ((uint8_t)out_steer)*((uint8_t)out_speed)); //simple checksum
if (out_checksum==0 || out_checksum==255){ out_checksum=1; } //cannot be 0 or 255 (special purpose)
if (!motorenabled){ //disable motors?
out_checksum=0; //checksum=0 disables motors
}
Serial2.write((uint8_t *) &out_steer, sizeof(out_steer));
Serial2.write((uint8_t *) &out_speed, sizeof(out_speed));
2019-03-16 19:02:27 +00:00
Serial2.write((uint8_t *) &out_checksum, sizeof(out_checksum));
last_send=millis();
#ifdef DEBUG
2019-03-16 19:02:27 +00:00
Serial.print(" steer=");
Serial.print(out_steer);
Serial.print(" speed=");
Serial.print(out_speed);
2019-03-16 19:02:27 +00:00
Serial.print(" checksum=");
Serial.print(out_checksum);
Serial.println();
2019-03-16 19:02:27 +00:00
#endif
}
}
2019-03-17 17:57:32 +00:00
void updateIMU()
{
2019-03-20 22:29:09 +00:00
if (old_ax==ax && old_ay==ay && old_az==az && old_gx==gx && old_gy==gy && old_gz==gz && old_mx==mx && old_my==my && old_mz==mz){
imu_no_change_counter++;
if (imu_no_change_counter>10){
error=IMU_NO_CHANGE;
Serial.println("Error: IMU_NO_CHANGE");
}
}else{
imu_no_change_counter=0;
}
old_ax=ax;
old_ay=ay;
old_az=az;
old_gx=gx;
old_gy=gy;
old_gz=gz;
old_mx=mx;
old_my=my;
old_mz=mz;
old_roll=roll;
old_pitch=pitch;
old_yaw=yaw;
2019-03-17 17:57:32 +00:00
//Update Imu and write to variables
imu.update();
imu.getAcceleration(&ax, &ay, &az);
imu.getGyro(&gx, &gy, &gz);
imu.getMag(&mx, &my, &mz,&ma); //calibration data such as bias is set in IMUGY85.h
roll = imu.getRoll();
pitch = imu.getPitch();
yaw = imu.getYaw();
/*Directions:
* Components on top.
* Roll: around Y axis (pointing to the right), left negative
* Pitch: around X axis (pointing forward), up positive
* Yaw: around Z axis, CCW positive, 0 to 360
*/
}