//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 //#define DEBUG 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 #define SENDPERIOD 20 //ms #define CONTROLUPDATEPERIOD 10 long last_controlupdate=0; #define IMUUPDATEPERIOD 10 //ms long last_imuupdated=0; #define MAX_YAWCHANGE 90 //in degrees, if exceeded in one update intervall error will be triggered #include //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; 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; double setYaw=0; float magalign_multiplier=0; //how much the magnetometer should influence steering, 0=none, 1=stay aligned //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 #include #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 }; #define NRF24CHANNEL 75 struct nrfdata { uint8_t steer; uint8_t speed; uint8_t commands; //bit 0 set = motor enable uint8_t checksum; }; nrfdata lastnrfdata; long last_nrfreceive=0; //last time values were received and checksum ok long nrf_delay=0; #define MAX_NRFDELAY 50 //command variables boolean motorenabled=false; //set by nrfdata.commands 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 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); digitalWrite(PIN_LED,HIGH); Serial.println("Initializing nrf24"); radio.begin(); radio.setDataRate( RF24_250KBPS ); //set to slow data rate. default was 1MBPS radio.setChannel(NRF24CHANNEL); //0 to 124 (inclusive) 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(); Serial.println("Initializing IMU"); imu.init(); Serial.println("Initialized"); } void loop() { if (millis()-last_imuupdated>IMUUPDATEPERIOD){ updateIMU(); last_imuupdated=millis(); } //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) ); if (lastnrfdata.speed==NRFDATA_CENTER && lastnrfdata.speed==NRFDATA_CENTER){ //arm only when centered armed=true; //arm at first received packet } //parse commands motorenabled = (lastnrfdata.commands & (1 << 0)); //check bit 0 if (!motorenabled){ //disable motors? armed=false; } 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 /* setYaw+=((int16_t)(lastnrfdata.steer)-NRFDATA_CENTER)*10/127; while (setYaw<0){ setYaw+=360; } while (setYaw>=360){ setYaw-=360; }*/ /* Serial.print("setYaw="); Serial.print(setYaw); Serial.print(" Yaw="); Serial.println(yaw);*/ } } if (error>0){ //disarm if error occured armed=false; } if (armed && nrf_delay>=MAX_NRFDELAY){ //too long since last sucessful nrf receive armed=false; } if (armed){ 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);*/ } }else{ //took too long since last nrf data out_steer=0; out_speed=0; setYaw=yaw; magalign_multiplier=0; } if (millis()-last_send>SENDPERIOD){ //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)); Serial2.write((uint8_t *) &out_checksum, sizeof(out_checksum)); last_send=millis(); #ifdef DEBUG Serial.print(" steer="); Serial.print(out_steer); Serial.print(" speed="); Serial.print(out_speed); Serial.print(" checksum="); Serial.print(out_checksum); Serial.println(); #endif } } void updateIMU() { 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; //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 */ }