#include //Arduino IDE Settings: //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 //Flashing the hoverbrett controller (bluepill): /* * connect uart adapter to serial port cable (the one with more red heatshrink) * (disconnect xt30 power connector) * set jumper on usb uart adapter to output 5V * hold boot0 button (black, the outermost) while powering up (or restarting with small button next to it) * flash */ //PA2 may be defective on my bluepill #define SERIAL_CONTROL_BAUD 115200 // [-] Baud rate for HoverSerial (used to communicate with the hoverboard) #define SERIAL_BAUD 115200 // [-] Baud rate for built-in Serial (used for the Serial Monitor) #define START_FRAME 0xABCD // [-] Start frme definition for reliable serial communication //#define DEBUG //#define PARAMETEROUTPUT //void ReceiveSerial2(); //void SendSerial2(int16_t uSpeedLeft, int16_t uSpeedRight); // Structs for serial communication typedef struct{ uint8_t idx = 0; // Index for new data pointer uint16_t bufStartFrame; // Buffer Start Frame byte *p; // Pointer declaration for the new received data byte incomingByte; byte incomingBytePrev; long lastValidDataSerial_time; } SerialRead; SerialRead Serialcom; typedef struct{ uint16_t start; int16_t speedLeft; int16_t speedRight; uint16_t checksum; } SerialCommand; SerialCommand Command; typedef struct{ //match this struct to hoverboard-firmware SerialFeedback struct in main.c uint16_t start; int16_t cmd1; int16_t cmd2; int16_t speedL_meas; //left speed is positive when driving forward int16_t speedR_meas; //right speed is negatie when driving forward int16_t batVoltage; int16_t boardTemp; int16_t curL_DC; //negative values are current consumed. positive values mean generated current int16_t curR_DC; uint16_t cmdLed; uint16_t checksum; } SerialFeedback; SerialFeedback FeedbackESC; SerialFeedback NewFeedbackESC; void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef); bool ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &NewFeedback, HardwareSerial &SerialRef); 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 //(offset-adcvalue)*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_speedbackward_p=0.7; float gt_steer_p=2.0; #define GT_SPEED_LIMIT 300 //maximum out_speed value + #define GT_SPEEDBACKWARD_LIMIT 100//maximum out_speed value (for backward driving) - #define GT_STEER_LIMIT 300 //maximum out_steer value +- #define GT_LENGTH_MAXIMUMDIFFBACKWARD -200 //[mm]. if gt_length_set=1000 and GT_LENGTH_MAXIMUMDIFFBACKWARD=-200 then only drives backward if lenght is greater 800 //#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 // Global variables /* uint8_t idx = 0; // Index for new data pointer uint16_t bufStartFrame; // Buffer Start Frame byte *p; // Pointer declaration for the new received data byte incomingByte; byte incomingBytePrev; */ void setup() { Serial1.begin(SERIAL_BAUD); //Debug and Program. A9=TX1, A10=RX1 (3v3 level) Serial3.begin(SERIAL_CONTROL_BAUD); //control. B10=TX3, B11=RX3 (Serial3 is Usart 3) //Serial2 may be dead on my board? analogReadResolution(12); //set resolution to 12 bit 0 - 4095 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 Serial1.println("Initializing nrf24"); #endif Serial1.println("radio begin"); radio.begin(); //Serial1.println("set rate"); radio.setDataRate( RF24_250KBPS ); //set to slow data rate. default was 1MBPS //radio.setDataRate( RF24_1MBPS ); //Serial1.println("set channel"); radio.setChannel(NRF24CHANNEL); //0 to 124 (inclusive) //Serial1.println("set retries and payload"); radio.setRetries(15, 15); // optionally, increase the delay between retries & # of retries radio.setPayloadSize(8); // optionally, reduce the payload size. seems to improve reliability //Serial1.println("open pipe"); radio.openWritingPipe(pipes[0]); //write on pipe 0 radio.openReadingPipe(1, pipes[1]); //read on pipe 1 //Serial1.println("start listening"); radio.startListening(); #ifdef DEBUG Serial1.println("Initialized"); #endif } void loop() { //ReceiveSerial2(); // Check for new received data bool newData=ReceiveSerial(Serialcom,FeedbackESC, NewFeedbackESC, Serial3); // Check for new received data 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_SCALE - (GT_LENGTH_SCALE*GT_LENGTH_OFFSET) +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)-((int16_t)GT_VERTICAL_CENTER), +GT_VERTICAL_RANGE,-GT_VERTICAL_RANGE,-127,127),-127,127); //left negative gt_horizontal = constrain(map(analogRead(PIN_GAMETRAK_HORIZONTAL)-((int16_t)GT_HORIZONTAL_CENTER), +GT_HORIZONTAL_RANGE,-GT_HORIZONTAL_RANGE,-127,127),-127,127); //down negative last_adcupdated = millis(); /* Serial1.print("gt_length="); Serial1.print(gt_length); Serial1.print(", gt_vertical="); Serial1.print(gt_vertical); Serial1.print(", gt_horizontal="); Serial1.println(gt_horizontal);*/ /* Serial1.print("PIN_GAMETRAK_LENGTH="); Serial1.print(analogRead(PIN_GAMETRAK_LENGTH)); Serial1.print(", PIN_GAMETRAK_VERTICAL="); Serial1.print(analogRead(PIN_GAMETRAK_VERTICAL)); Serial1.print(", PIN_GAMETRAK_HORIZONTAL="); Serial1.println(analogRead(PIN_GAMETRAK_HORIZONTAL));*/ } //NRF24 nrf_delay = millis() - last_nrfreceive; //update nrf delay if ( radio.available() ) { //Serial1.println("radio available ..."); lastpacketOK = false; //initialize with false, if checksum ok gets set to true digitalWrite(PIN_LED, !digitalRead(PIN_LED)); 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 Serial1.print("Received:"); Serial1.print(" st="); Serial1.print(lastnrfdata.steer); Serial1.print(", sp="); Serial1.print(lastnrfdata.speed); Serial1.print(", c="); Serial1.print(lastnrfdata.commands); Serial1.print(", chks="); Serial1.print(lastnrfdata.checksum); Serial1.print("nrfdelay="); Serial1.print(nrf_delay); Serial1.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; }*/ /* Serial1.print("setYaw="); Serial1.print(setYaw); Serial1.print(" Yaw="); Serial1.println(yaw);*/ } if (controlmode == MODE_RADIONRF && nrf_delay >= MAX_NRFDELAY) { //too long since last sucessful nrf receive controlmode = MODE_DISARMED; #ifdef DEBUG Serial1.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; */ //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, -1500, 1500); out_speedr = constrain(set_speed * SPEED_COEFFICIENT_NRF - set_steer * STEER_COEFFICIENT_NRF, -1500, 1500); /* Serial1.print("Out steer="); Serial1.println(out_steer);*/ } }//if pastpacket not ok, keep last out_steer and speed values until disarmed #ifdef DEBUG if (!lastpacketOK) { Serial1.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_diff0) { //needs to drive forward set_speed = constrain((int16_t)(_gt_length_diff*gt_speed_p),0,GT_SPEED_LIMIT); }else{ //drive backward if (_gt_length_diff > GT_LENGTH_MAXIMUMDIFFBACKWARD){ //only drive if not pulled back too much set_speed = constrain((int16_t)(_gt_length_diff*gt_speedbackward_p),-GT_SPEEDBACKWARD_LIMIT,0); }else{ set_speed = 0; //stop set_steer = 0; } } //calculate speed l and r from speed and steer #define SPEED_COEFFICIENT_GT 1 // higher value == stronger #define STEER_COEFFICIENT_GT 0.5 // higher value == stronger out_speedl = constrain(set_speed * SPEED_COEFFICIENT_GT + set_steer * STEER_COEFFICIENT_GT, -1000, 1000); out_speedr = constrain(set_speed * SPEED_COEFFICIENT_GT - set_steer * STEER_COEFFICIENT_GT, -1000, 1000); } if (error > 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 } if (motorenabled) { //motors enabled //SendSerial2(out_speedl,out_speedr); SendSerial(Command,out_speedl,out_speedr,Serial3); } else { //motors disabled //SendSerial2(0,0); SendSerial(Command,0,0,Serial3); } lastsend_out_speedl = out_speedl; //remember last transmittet values (for stat sending) lastsend_out_speedr = out_speedr; last_send = millis(); #ifdef DEBUG Serial1.print(" out_speedl="); Serial1.print(out_speedl); Serial1.print(" out_speedr="); Serial1.print(out_speedr); Serial1.print(" checksum="); Serial1.print(out_checksum); Serial1.print(" controlmode="); Serial1.print(controlmode); Serial1.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 Serial1.println("Transmitting..."); #endif radio.stopListening(); //stop listening to be able to transmit radiosendOk = radio.write( &senddata, sizeof(nrfstatdata) ); if (!radiosendOk){ #ifdef DEBUG Serial1.println("send failed"); #endif } radio.startListening(); //start listening again } */ // ########################## SEND ########################## void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef) { // Create command scom.start = (uint16_t)START_FRAME; scom.speedLeft = (int16_t)uSpeedLeft; scom.speedRight = (int16_t)uSpeedRight; scom.checksum = (uint16_t)(scom.start ^ scom.speedLeft ^ scom.speedRight); SerialRef.write((uint8_t *) &scom, sizeof(scom)); } /* void SendSerial2(int16_t uSpeedLeft, int16_t uSpeedRight) { // Create command Command.start = (uint16_t)START_FRAME; Command.speedLeft = (int16_t)uSpeedLeft; Command.speedRight = (int16_t)uSpeedRight; Command.checksum = (uint16_t)(Command.start ^ Command.speedLeft ^ Command.speedRight); // Write to Serial Serial2.write((uint8_t *) &Command, sizeof(Command)); }*/ // ########################## RECEIVE ########################## bool ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &NewFeedback, HardwareSerial &SerialRef) { bool _result=1; // Check for new data availability in the Serial buffer if ( SerialRef.available() ) { sread.incomingByte = SerialRef.read(); // Read the incoming byte sread.bufStartFrame = ((uint16_t)(sread.incomingByte) << 8) | sread.incomingBytePrev; // Construct the start frame } else { return 0; } // If DEBUG_RX is defined print all incoming bytes #ifdef DEBUG_RX Serial.print(sread.incomingByte); #endif // Copy received data if (sread.bufStartFrame == START_FRAME) { // Initialize if new data is detected sread.p = (byte *)&NewFeedback; *sread.p++ = sread.incomingBytePrev; *sread.p++ = sread.incomingByte; sread.idx = 2; } else if (sread.idx >= 2 && sread.idx < sizeof(SerialFeedback)) { // Save the new received data *sread.p++ = sread.incomingByte; sread.idx++; } // Check if we reached the end of the package if (sread.idx == sizeof(SerialFeedback)) { uint16_t checksum; checksum = (uint16_t)(NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.cmd2 ^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp ^ NewFeedback.curL_DC ^ NewFeedback.curR_DC ^ NewFeedback.cmdLed); // Check validity of the new data if (NewFeedback.start == START_FRAME && checksum == NewFeedback.checksum) { // Copy the new data memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback)); sread.lastValidDataSerial_time = millis(); } else { _result=0; } sread.idx = 0; // Reset the index (it prevents to enter in this if condition in the next cycle) } /* // Print data to built-in Serial Serial1.print("1: "); Serial.print(Feedback.cmd1); Serial1.print(" 2: "); Serial.print(Feedback.cmd2); Serial1.print(" 3: "); Serial.print(Feedback.speedR); Serial1.print(" 4: "); Serial.print(Feedback.speedL); Serial1.print(" 5: "); Serial.print(Feedback.speedR_meas); Serial1.print(" 6: "); Serial.print(Feedback.speedL_meas); Serial1.print(" 7: "); Serial.print(Feedback.batVoltage); Serial1.print(" 8: "); Serial.println(Feedback.boardTemp); } else { Serial1.println("Non-valid data skipped"); }*/ // Update previous states sread.incomingBytePrev = sread.incomingByte; return _result; //new data was available } /* void ReceiveSerial2() { // Check for new data availability in the Serial buffer if (Serial2.available()) { incomingByte = Serial2.read(); // Read the incoming byte bufStartFrame = ((uint16_t)(incomingBytePrev) << 8) + incomingByte; // Construct the start frame } else { return; } // If DEBUG_RX is defined print all incoming bytes #ifdef DEBUG_RX Serial.print(incomingByte); return; #endif // Copy received data if (bufStartFrame == START_FRAME) { // Initialize if new data is detected p = (byte *)&NewFeedback; *p++ = incomingBytePrev; *p++ = incomingByte; idx = 2; } else if (idx >= 2 && idx < sizeof(SerialFeedback)) { // Save the new received data *p++ = incomingByte; idx++; } // Check if we reached the end of the package if (idx == sizeof(SerialFeedback)) { uint16_t checksum; checksum = (uint16_t)(NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.cmd2 ^ NewFeedback.speedR ^ NewFeedback.speedL ^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp ^ NewFeedback.curL_DC ^ NewFeedback.curR_DC); // Check validity of the new data if (NewFeedback.start == START_FRAME && checksum == NewFeedback.checksum) { // Copy the new data memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback)); // Print data to built-in Serial Serial.print("1: "); Serial.print(Feedback.cmd1); Serial.print(" 2: "); Serial.print(Feedback.cmd2); Serial.print(" 3: "); Serial.print(Feedback.speedR); Serial.print(" 4: "); Serial.print(Feedback.speedL); Serial.print(" 5: "); Serial.print(Feedback.speedR_meas); Serial.print(" 6: "); Serial.print(Feedback.speedL_meas); //Serial.print(" 7: "); Serial.print(Feedback.batVoltage); //Serial.print(" 8: "); Serial.println(Feedback.boardTemp); Serial.print(" 9: "); Serial.print(Feedback.curL_DC); //in A, in hoverbrett negative sign for forward Serial.print(" 10: "); Serial.println(Feedback.curR_DC); //in A, in hoverbrett negative sign for forward Serial.print(" 9: "); Serial.println(Feedback.curL_DC); //in A, in hoverbrett negative sign for forward } else { Serial.println("Non-valid data skipped"); } idx = 0; // Reset the index (it prevents to enter in this if condition in the next cycle) } // Update previous states incomingBytePrev = incomingByte; } */