//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 //PA2 may be defective on my bluepill //#define DEBUG #define PARAMETEROUTPUT 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 PIN_VBAT PA0 //battery voltage after voltage divider //#define VBAT_DIV_FACTOR 0.010700 //how much voltage (V) equals one adc unit. measured at 40V and averaged #define VBAT_DIV_FACTOR 0.01399535423925667828 //how much voltage (V) equals one adc unit. 3444=48.2V #define PIN_CURRENT PA1 //output of hall sensor for current measurement #define CURRENT_OFFSET 2048 //adc reading at 0A, with CJMCU-758 typically at Vcc/2. measured with actual voltage supply in hoverbrett #define CURRENT_FACTOR 0.38461538461538461538 //how much current (A) equals one adc unit. 2045-2032=13 at 5A float vbat=0; //battery voltage float ibat=0; //battery current long last_adcupdated=0; #define ADC_UPDATEPERIOD 10 //in ms #define SENDPERIOD 20 //ms. delay for sending speed and steer data to motor controller via serial //Status information sending #define PARAMETERSENDPERIOD 50 //delay for sending stat data via nrf24 long last_parametersend=0; #define CONTROLUPDATEPERIOD 10 long last_controlupdate = 0; #define PIN_GAMETRAK_LENGTH PA1 //yellow (connector) / orange (gametrak module wires): length #define PIN_GAMETRAK_VERTICAL PA3 //orange / red: vertical #define PIN_GAMETRAK_HORIZONTAL PA4 //blue / yellow: horizontal #define GT_LENGTH_OFFSET 4090 //adc offset value (rolled up value) #define GT_LENGTH_MIN 220 //length in mm at which adc values start to change #define GT_LENGTH_SCALE -0.73 //(adcvalue-offset)*scale = length[mm] (+length_min) //2720 at 1000mm+220mm -> 1370 for 1000mm -> #define GT_LENGTH_MAXLENGTH 2500 //maximum length in [mm]. maximum string length is around 2m80 uint16_t gt_length=0; //0=rolled up, 1unit = 1mm #define GT_VERTICAL_CENTER 2048 //adc value for center position #define GT_VERTICAL_RANGE 2047 //adc value difference from center to maximum (30 deg) int8_t gt_vertical=0; //0=center. joystick can rotate +-30 degrees. -127 = -30 deg //left = -30 deg, right= 30deg #define GT_HORIZONTAL_CENTER 2048 //adc value for center position #define GT_HORIZONTAL_RANGE 2047 //adc value difference from center to maximum (30 deg) int8_t gt_horizontal=0; //0=center uint16_t gt_length_set=1000; //set length to keep [mm] #define GT_LENGTH_MINDIFF 10 //[mm] threshold, do not move within gt_length_set-GT_LENGTH_MINDIFF and gt_length_set+GT_LENGTH_MINDIFF float gt_speed_p=0.7; //value to multipy difference [mm] with -> out_speed float gt_steer_p=2.0; #define GT_SPEED_LIMIT 300 //maximum out_speed value +- #define GT_STEER_LIMIT 300 //maximum out_steer value +- #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 #define IMUUPDATEPERIOD 10 //ms long last_imuupdated = 0; #define MAX_YAWCHANGE 90 //in degrees, if exceeded in one update intervall error will be triggered 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 // Lenovo Trackpoint pinout //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 100 //ms. maximum time delay at which vehicle will disarm boolean radiosendOk=false; //command variables boolean motorenabled = false; //set by nrfdata.commands long last_send = 0; int16_t out_speedl = 0; //between -1000 and 1000 int16_t out_speedr = 0; int16_t lastsend_out_speedl = 0; //last value transmitted to motor controller int16_t lastsend_out_speedr = 0; int16_t set_speed = 0; int16_t set_steer = 0; uint8_t out_checksum = 0; //0= disable motors, 255=reserved, 1<=checksum<255 #define NRFDATA_CENTER 127 //boolean armed = false; boolean lastpacketOK = false; //Gametrak //boolean armed_gt = false; uint8_t controlmode=0; #define MODE_DISARMED 0 #define MODE_RADIONRF 1 #define MODE_GAMETRAK 2 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) //Serial1 max be dead on my board? pinMode(PIN_LED, OUTPUT); digitalWrite(PIN_LED, HIGH); pinMode(PIN_VBAT,INPUT_ANALOG); pinMode(PIN_CURRENT,INPUT_ANALOG); pinMode(PIN_GAMETRAK_LENGTH,INPUT_ANALOG); pinMode(PIN_GAMETRAK_VERTICAL,INPUT_ANALOG); pinMode(PIN_GAMETRAK_HORIZONTAL,INPUT_ANALOG); #ifdef DEBUG Serial.println("Initializing nrf24"); #endif radio.begin(); radio.setDataRate( RF24_250KBPS ); //set to slow data rate. default was 1MBPS //radio.setDataRate( RF24_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(); #ifdef DEBUG Serial.println("Initializing IMU"); #endif imu.init(); #ifdef DEBUG Serial.println("Initialized"); #endif } void loop() { if (millis() - last_imuupdated > IMUUPDATEPERIOD) { updateIMU(); last_imuupdated = millis(); } if (millis() - last_adcupdated > ADC_UPDATEPERIOD) { //update analog readings vbat=analogRead(PIN_VBAT)*VBAT_DIV_FACTOR; ibat=(analogRead(PIN_CURRENT)-CURRENT_OFFSET)*CURRENT_FACTOR; gt_length = constrain((analogRead(PIN_GAMETRAK_LENGTH)-GT_LENGTH_OFFSET)*GT_LENGTH_SCALE +GT_LENGTH_MIN, 0,GT_LENGTH_MAXLENGTH); if (gt_length<=GT_LENGTH_MIN){ gt_length=0; //if below minimum measurable length set to 0mm } gt_vertical = constrain(map(analogRead(PIN_GAMETRAK_VERTICAL)-GT_VERTICAL_CENTER, +GT_VERTICAL_RANGE,-GT_VERTICAL_RANGE,-127,127),-127,127); //left negative gt_horizontal = constrain(map(analogRead(PIN_GAMETRAK_HORIZONTAL)-GT_HORIZONTAL_CENTER, +GT_HORIZONTAL_RANGE,-GT_HORIZONTAL_RANGE,-127,127),-127,127); //down negative last_adcupdated = millis(); /* Serial.print("gt_length="); Serial.print(gt_length); Serial.print(", gt_vertical="); Serial.print(gt_vertical); Serial.print(", gt_horizontal="); Serial.println(gt_horizontal);*/ /* Serial.print("PIN_GAMETRAK_LENGTH="); Serial.print(analogRead(PIN_GAMETRAK_LENGTH)); Serial.print(", PIN_GAMETRAK_VERTICAL="); Serial.print(analogRead(PIN_GAMETRAK_VERTICAL)); Serial.print(", PIN_GAMETRAK_HORIZONTAL="); Serial.println(analogRead(PIN_GAMETRAK_HORIZONTAL)); */ } //NRF24 nrf_delay = millis() - last_nrfreceive; //update nrf delay if ( radio.available() ) { //Serial.println("radio available ..."); bool done = false; while (!done) { lastpacketOK = false; //initialize with false, if checksum ok gets set to true digitalWrite(PIN_LED, !digitalRead(PIN_LED)); done = radio.read( &lastnrfdata, sizeof(nrfdata) ); if (lastnrfdata.speed == NRFDATA_CENTER && lastnrfdata.steer == NRFDATA_CENTER) { //arm only when centered controlmode = MODE_RADIONRF;//set radionrf mode at first received packet } uint8_t calcchecksum = (uint8_t)((lastnrfdata.steer + 3) * (lastnrfdata.speed + 13)); if (lastnrfdata.checksum == calcchecksum) { //checksum ok? lastpacketOK = true; last_nrfreceive = millis(); //parse commands motorenabled = (lastnrfdata.commands & (1 << 0))>>0; //check bit 0 } /* #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); Serial.println(); #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 (controlmode == MODE_RADIONRF && nrf_delay >= MAX_NRFDELAY) { //too long since last sucessful nrf receive controlmode = MODE_DISARMED; #ifdef DEBUG Serial.println("nrf_delay>=MAX_NRFDELAY, disarmed!"); #endif } if (controlmode == MODE_RADIONRF) { //is armed in nrf mode if (lastpacketOK) { //if lastnrfdata is valid 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 ); set_speed = (int16_t)( ((int16_t)(lastnrfdata.speed) - NRFDATA_CENTER) * 1000 / 127 ); //-1000 to 1000 set_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 set_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 = 0; //Force mag off 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 set_steer = set_steer * (1 - magalign_multiplier) + set_steer_mag * magalign_multiplier; setYaw = setYaw * magalign_multiplier + yaw * (1 - magalign_multiplier); //if magalign_multiplier 0, setYaw equals current yaw //calculate speed l and r from speed and steer #define SPEED_COEFFICIENT_NRF 1 // higher value == stronger #define STEER_COEFFICIENT_NRF 0.5 // higher value == stronger out_speedl = constrain(set_speed * SPEED_COEFFICIENT_NRF + set_steer * STEER_COEFFICIENT_NRF, -1000, 1000); out_speedr = constrain(set_speed * SPEED_COEFFICIENT_NRF - set_steer * STEER_COEFFICIENT_NRF, -1000, 1000); /* Serial.print("Out steer="); Serial.println(out_steer);*/ } }//if pastpacket not ok, keep last out_steer and speed values until disarmed if (!motorenabled) { //radio connected but not actively driving, keep values reset setYaw = yaw; magalign_multiplier = 0; } #ifdef DEBUG if (!lastpacketOK) { Serial.println("Armed but packet not ok"); } #endif } if (controlmode==MODE_DISARMED) { //check if gametrak can be armed if (gt_length>gt_length_set && gt_length-GT_LENGTH_MINDIFF & _gt_length_diff 0) { //disarm if error occured controlmode = MODE_DISARMED; //force disarmed } if (controlmode == MODE_DISARMED){ //all disarmed out_speedl = 0; out_speedr = 0; } if (millis() - last_send > SENDPERIOD) { //calculate checksum out_checksum = ((uint8_t) ((uint8_t)out_speedl) * ((uint8_t)out_speedr)); //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_speedl, sizeof(out_speedl)); Serial2.write((uint8_t *) &out_speedr, sizeof(out_speedr)); Serial2.write((uint8_t *) &out_checksum, sizeof(out_checksum)); lastsend_out_speedl = out_speedl; //remember last transmittet values (for stat sending) lastsend_out_speedr = out_speedr; last_send = millis(); #ifdef DEBUG Serial.print(" out_speedl="); Serial.print(out_speedl); Serial.print(" out_speedr="); Serial.print(out_speedr); Serial.print(" checksum="); Serial.print(out_checksum); Serial.print(" controlmode="); Serial.print(controlmode); Serial.println(); #endif } // #ifdef PARAMETEROUTPUT if ( millis() - last_parametersend > PARAMETERSENDPERIOD) { //Serial.write((uint8_t *) &counter, sizeof(counter));//uint8_t, 1 byte //Serial.write((uint8_t *) &value1, sizeof(value1)); //uint16_t, 2 bytes //Serial.write((uint8_t *) &value2, sizeof(value2)); //int16_t, 2 bytes //Serial.write((uint8_t *) &floatvalue, sizeof(floatvalue)); //float, 4 bytes uint8_t booleanvalues=0; //reset booleanvalues |= motorenabled<<0; //bit 0 booleanvalues |= (controlmode&0b00000011)<<1; //bit 1 and 2 (2bit number for controlmodes (3) Serial.write((uint8_t *) &out_speedl, sizeof(out_speedl)); //int16_t, 2 bytes Serial.write((uint8_t *) &out_speedr, sizeof(out_speedr)); //int16_t, 2 bytes Serial.write((uint8_t *) &booleanvalues, sizeof(booleanvalues)); //uint8_t, 1 byte //booleanvalues Serial.write((uint8_t *) &vbat, sizeof(vbat)); //float, 4 bytes //Serial.write((uint8_t *) &ibat, sizeof(ibat)); //float, 4 bytes float yaw_float=yaw; Serial.write((uint8_t *) &yaw_float, sizeof(yaw_float)); //float, 4 bytes Serial.write((uint8_t *) >_length, sizeof(gt_length)); //uint16_t, 2 bytes Serial.write((uint8_t *) >_horizontal, sizeof(gt_horizontal)); //int8_t, 1 byte Serial.write((uint8_t *) >_vertical, sizeof(gt_vertical)); //int8_t, 1 byte last_parametersend = millis(); } #endif } /* void sendRF(nrfstatdata senddata){ #ifdef DEBUG Serial.println("Transmitting..."); #endif radio.stopListening(); //stop listening to be able to transmit radiosendOk = radio.write( &senddata, sizeof(nrfstatdata) ); if (!radiosendOk){ #ifdef DEBUG Serial.println("send failed"); #endif } radio.startListening(); //start listening again } */ 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; #ifdef DEBUG Serial.println("Error: IMU_NO_CHANGE"); #endif } } 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 */ }