From 7c4c4c693a3e6e1c1db30ede31bd213debcc0d0a Mon Sep 17 00:00:00 2001 From: Fisch Date: Sun, 27 Mar 2022 12:05:06 +0200 Subject: [PATCH] update all controllers --- controller/controller.ino | 696 ------------------- controller_pio/.gitignore | 5 + controller_pio/.vscode/c_cpp_properties.json | 142 ---- controller_pio/.vscode/extensions.json | 17 +- controller_pio/.vscode/launch.json | 34 - controller_pio/platformio.ini | 11 +- controller_pio/src/main.cpp | 304 +++++--- hoverboard-firmware-hack-foc-serial-esc | 2 +- hoverboard-firmware-hack-serial-esc | 1 - nippleremote | 2 +- 10 files changed, 234 insertions(+), 980 deletions(-) delete mode 100644 controller/controller.ino delete mode 100644 controller_pio/.vscode/c_cpp_properties.json delete mode 100644 controller_pio/.vscode/launch.json delete mode 160000 hoverboard-firmware-hack-serial-esc diff --git a/controller/controller.ino b/controller/controller.ino deleted file mode 100644 index 37e8c7c..0000000 --- a/controller/controller.ino +++ /dev/null @@ -1,696 +0,0 @@ -//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: -/* - * 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 38400 // [-] 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 0xAAAA // [-] Start frme definition for reliable serial communication - -//#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_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; - -typedef struct{ - uint16_t start; - int16_t speedLeft; - int16_t speedRight; - uint16_t checksum; -} SerialCommand; -SerialCommand Command; - -typedef struct{ - uint16_t start; - int16_t cmd1; - int16_t cmd2; - int16_t speedL; - int16_t speedR; - int16_t speedL_meas; - int16_t speedR_meas; - int16_t batVoltage; - int16_t boardTemp; - int16_t curL_DC; - int16_t curR_DC; - uint16_t checksum; -} SerialFeedback; -SerialFeedback Feedback; -SerialFeedback NewFeedback; - - - - -void setup() { - - Serial.begin(SERIAL_BAUD); //Debug and Program. A9=TX1, A10=RX1 (3v3 level) - - Serial2.begin(SERIAL_CONTROL_BAUD); //control. B10=TX3, B11=RX3 (Serial2 is Usart 3) - //Serial1 may 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() { - - ReceiveSerial2(); // Check for new received data - - 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, -1500, 1500); - out_speedr = constrain(set_speed * SPEED_COEFFICIENT_NRF - set_steer * STEER_COEFFICIENT_NRF, -1500, 1500); - - /* - 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_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 - } - - /*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));*/ - if (motorenabled) { //motors enabled - SendSerial2(out_speedl,out_speedr); - } else { //motors disabled - SendSerial2(0,0); - } - 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 - */ -} - - -// ########################## SEND ########################## -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 ########################## -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 - } 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; -} diff --git a/controller_pio/.gitignore b/controller_pio/.gitignore index 03f4a3c..d2edddf 100644 --- a/controller_pio/.gitignore +++ b/controller_pio/.gitignore @@ -1 +1,6 @@ .pio +.vscode/.browse.c_cpp.db* +.vscode/c_cpp_properties.json +.vscode/launch.json +.vscode/ipch + diff --git a/controller_pio/.vscode/c_cpp_properties.json b/controller_pio/.vscode/c_cpp_properties.json deleted file mode 100644 index 33db116..0000000 --- a/controller_pio/.vscode/c_cpp_properties.json +++ /dev/null @@ -1,142 +0,0 @@ -{ - "configurations": [ - { - "name": "!!! WARNING !!! AUTO-GENERATED FILE, PLEASE DO NOT MODIFY IT AND USE https://docs.platformio.org/page/projectconf/section_env_build.html#build-flags" - }, - { - "name": "Linux", - "includePath": [ - "/media/fisch/HDD/Projects/hoverbrett/controller_pio/include", - "/media/fisch/HDD/Projects/hoverbrett/controller_pio/src", - "/media/fisch/HDD/Projects/hoverbrett/controller_pio/.pio/libdeps/genericSTM32F103C8/RF24_ID433", - "/media/fisch/HDD/Projects/hoverbrett/controller_pio/.pio/libdeps/genericSTM32F103C8/RF24_ID433/utility", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/SPI/src", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/cores/maple", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/system/libmaple", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/system/libmaple/include", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/system/libmaple/stm32f1/include", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/system/libmaple/usb/stm32f1", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/system/libmaple/usb/usb_lib", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/variants/generic_stm32f103c", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/A_STM32_Examples", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Adafruit_GFX_AS", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Adafruit_ILI9341", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Adafruit_ILI9341_STM", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Adafruit_SSD1306", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/EEPROM", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Ethernet_STM/src", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/FreeRTOS701", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/FreeRTOS701/utility", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/FreeRTOS821", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/FreeRTOS821/utility", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/FreeRTOS900", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Lcd7920_STM", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/LiquidCrystal", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/MapleCoOS", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/MapleCoOS/utility", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/MapleCoOS116", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/MapleCoOS116/utility", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/OLED_I2C", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/OneWireSTM/src", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/RTClock/src", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/SDIO", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/STM32ADC/src", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Serasidis_EtherCard_STM/src", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Serasidis_VS1003B_STM/src", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Serasidis_XPT2046_touch/src", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Servo/src", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Touch-Screen-Library_STM", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/USBComposite", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/WS2812B/src", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Wire", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Wire/utility", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/WireSlave/src", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/stm_fft", - "/home/fisch/.platformio/packages/tool-unity", - "" - ], - "browse": { - "limitSymbolsToIncludedHeaders": true, - "path": [ - "/media/fisch/HDD/Projects/hoverbrett/controller_pio/include", - "/media/fisch/HDD/Projects/hoverbrett/controller_pio/src", - "/media/fisch/HDD/Projects/hoverbrett/controller_pio/.pio/libdeps/genericSTM32F103C8/RF24_ID433", - "/media/fisch/HDD/Projects/hoverbrett/controller_pio/.pio/libdeps/genericSTM32F103C8/RF24_ID433/utility", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/SPI/src", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/cores/maple", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/system/libmaple", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/system/libmaple/include", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/system/libmaple/stm32f1/include", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/system/libmaple/usb/stm32f1", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/system/libmaple/usb/usb_lib", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/variants/generic_stm32f103c", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/A_STM32_Examples", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Adafruit_GFX_AS", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Adafruit_ILI9341", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Adafruit_ILI9341_STM", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Adafruit_SSD1306", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/EEPROM", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Ethernet_STM/src", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/FreeRTOS701", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/FreeRTOS701/utility", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/FreeRTOS821", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/FreeRTOS821/utility", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/FreeRTOS900", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Lcd7920_STM", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/LiquidCrystal", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/MapleCoOS", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/MapleCoOS/utility", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/MapleCoOS116", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/MapleCoOS116/utility", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/OLED_I2C", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/OneWireSTM/src", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/RTClock/src", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/SDIO", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/STM32ADC/src", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Serasidis_EtherCard_STM/src", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Serasidis_VS1003B_STM/src", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Serasidis_XPT2046_touch/src", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Servo/src", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Touch-Screen-Library_STM", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/USBComposite", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/WS2812B/src", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Wire", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Wire/utility", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/WireSlave/src", - "/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/stm_fft", - "/home/fisch/.platformio/packages/tool-unity", - "" - ] - }, - "defines": [ - "PLATFORMIO=40304", - "STM32F103xB", - "STM32F1", - "ARDUINO=10808", - "ARDUINO_ARCH_STM32", - "ARDUINO_ARCH_STM32F1", - "ARDUINO_GENERIC_STM32F103C", - "MCU_STM32F103C8", - "__STM32F1__", - "BOARD_generic_stm32f103c", - "F_CPU=72000000L", - "VECT_TAB_ADDR=0x8000000", - "ERROR_LED_PORT=GPIOC", - "ERROR_LED_PIN=13", - "CONFIG_MAPLE_MINI_NO_DISABLE_DEBUG=1", - "" - ], - "intelliSenseMode": "clang-x64", - "cStandard": "c11", - "cppStandard": "c++11", - "compilerPath": "/home/fisch/.platformio/packages/toolchain-gccarmnoneeabi@1.70201.0/bin/arm-none-eabi-gcc", - "compilerArgs": [ - "-mcpu=cortex-m3", - "-mthumb", - "-march=armv7-m", - "" - ] - } - ], - "version": 4 -} diff --git a/controller_pio/.vscode/extensions.json b/controller_pio/.vscode/extensions.json index 0f0d740..080e70d 100644 --- a/controller_pio/.vscode/extensions.json +++ b/controller_pio/.vscode/extensions.json @@ -1,7 +1,10 @@ -{ - // See http://go.microsoft.com/fwlink/?LinkId=827846 - // for the documentation about the extensions.json format - "recommendations": [ - "platformio.platformio-ide" - ] -} +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ], + "unwantedRecommendations": [ + "ms-vscode.cpptools-extension-pack" + ] +} diff --git a/controller_pio/.vscode/launch.json b/controller_pio/.vscode/launch.json deleted file mode 100644 index 4d6ddbc..0000000 --- a/controller_pio/.vscode/launch.json +++ /dev/null @@ -1,34 +0,0 @@ -// AUTOMATICALLY GENERATED FILE. PLEASE DO NOT MODIFY IT MANUALLY - -// PIO Unified Debugger -// -// Documentation: https://docs.platformio.org/page/plus/debugging.html -// Configuration: https://docs.platformio.org/page/projectconf/section_env_debug.html - -{ - "version": "0.2.0", - "configurations": [ - { - "type": "platformio-debug", - "request": "launch", - "name": "PIO Debug", - "executable": "/media/fisch/HDD/Projects/hoverbrett/controller_pio/.pio/build/genericSTM32F103C8/firmware.elf", - "toolchainBinDir": "/home/fisch/.platformio/packages/toolchain-gccarmnoneeabi@1.70201.0/bin", - "svdPath": "/home/fisch/.platformio/platforms/ststm32/misc/svd/STM32F103xx.svd", - "preLaunchTask": { - "type": "PlatformIO", - "task": "Pre-Debug" - }, - "internalConsoleOptions": "openOnSessionStart" - }, - { - "type": "platformio-debug", - "request": "launch", - "name": "PIO Debug (skip Pre-Debug)", - "executable": "/media/fisch/HDD/Projects/hoverbrett/controller_pio/.pio/build/genericSTM32F103C8/firmware.elf", - "toolchainBinDir": "/home/fisch/.platformio/packages/toolchain-gccarmnoneeabi@1.70201.0/bin", - "svdPath": "/home/fisch/.platformio/platforms/ststm32/misc/svd/STM32F103xx.svd", - "internalConsoleOptions": "openOnSessionStart" - } - ] -} \ No newline at end of file diff --git a/controller_pio/platformio.ini b/controller_pio/platformio.ini index 0aaa5cc..983eac4 100644 --- a/controller_pio/platformio.ini +++ b/controller_pio/platformio.ini @@ -8,6 +8,8 @@ ; Please visit documentation for the other options and examples ; https://docs.platformio.org/page/projectconf.html +;RF24@>=1.3.4 <- vorher benutzt + [env:genericSTM32F103C8] platform = ststm32 board = genericSTM32F103C8 @@ -17,5 +19,12 @@ upload_protocol = serial monitor_speed = 115200 +build_flags = + -D HAVE_HWSERIAL1 + -D HAVE_HWSERIAL3 + lib_deps = - RF24@>=1.3.4 \ No newline at end of file + https://github.com/maniacbug/RF24 + + + \ No newline at end of file diff --git a/controller_pio/src/main.cpp b/controller_pio/src/main.cpp index a23a1f1..d8888b1 100644 --- a/controller_pio/src/main.cpp +++ b/controller_pio/src/main.cpp @@ -1,8 +1,7 @@ #include -void ReceiveSerial2(); -void SendSerial2(int16_t uSpeedLeft, int16_t uSpeedRight); +//Arduino IDE Settings: //https://github.com/rogerclarkmelbourne/Arduino_STM32 in arduino/hardware //Board: Generic STM32F103C series //Upload method: serial @@ -12,7 +11,7 @@ void SendSerial2(int16_t uSpeedLeft, int16_t uSpeedRight); //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: +//Flashing the hoverbrett controller (bluepill): /* * connect uart adapter to serial port cable (the one with more red heatshrink) * (disconnect xt30 power connector) @@ -23,12 +22,54 @@ void SendSerial2(int16_t uSpeedLeft, int16_t uSpeedRight); //PA2 may be defective on my bluepill -#define SERIAL_CONTROL_BAUD 38400 // [-] Baud rate for HoverSerial (used to communicate with the hoverboard) +#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 0xAAAA // [-] Start frme definition for reliable serial communication +#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; @@ -63,7 +104,7 @@ long last_controlupdate = 0; #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) +#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 @@ -179,48 +220,26 @@ uint8_t controlmode=0; // 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; - -typedef struct{ - uint16_t start; - int16_t speedLeft; - int16_t speedRight; - uint16_t checksum; -} SerialCommand; -SerialCommand Command; - -typedef struct{ - uint16_t start; - int16_t cmd1; - int16_t cmd2; - int16_t speedL; - int16_t speedR; - int16_t speedL_meas; - int16_t speedR_meas; - int16_t batVoltage; - int16_t boardTemp; - int16_t curL_DC; - int16_t curR_DC; - uint16_t checksum; -} SerialFeedback; -SerialFeedback Feedback; -SerialFeedback NewFeedback; +*/ void setup() { - Serial.begin(SERIAL_BAUD); //Debug and Program. A9=TX1, A10=RX1 (3v3 level) + Serial1.begin(SERIAL_BAUD); //Debug and Program. A9=TX1, A10=RX1 (3v3 level) - Serial2.begin(SERIAL_CONTROL_BAUD); //control. B10=TX3, B11=RX3 (Serial2 is Usart 3) - //Serial1 may be dead on my board? + 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); @@ -235,32 +254,37 @@ void setup() { #ifdef DEBUG - Serial.println("Initializing nrf24"); + 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 - Serial.println("Initialized"); + Serial1.println("Initialized"); #endif @@ -269,7 +293,8 @@ void setup() { void loop() { - ReceiveSerial2(); // Check for new received data + //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 @@ -277,38 +302,39 @@ void loop() { 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); + 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)-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 + 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(); - /* - 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)); - */ + 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() ) { - //Serial.println("radio available ..."); + //Serial1.println("radio available ..."); lastpacketOK = false; //initialize with false, if checksum ok gets set to true digitalWrite(PIN_LED, !digitalRead(PIN_LED)); @@ -332,19 +358,19 @@ void loop() { /* #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); + 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); - Serial.print("nrfdelay="); - Serial.print(nrf_delay); - Serial.println(); + Serial1.print("nrfdelay="); + Serial1.print(nrf_delay); + Serial1.println(); #endif */ @@ -361,10 +387,10 @@ void loop() { }*/ /* - Serial.print("setYaw="); - Serial.print(setYaw); - Serial.print(" Yaw="); - Serial.println(yaw);*/ + Serial1.print("setYaw="); + Serial1.print(setYaw); + Serial1.print(" Yaw="); + Serial1.println(yaw);*/ } @@ -375,7 +401,7 @@ void loop() { 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!"); + Serial1.println("nrf_delay>=MAX_NRFDELAY, disarmed!"); #endif } if (controlmode == MODE_RADIONRF) { //is armed in nrf mode @@ -421,15 +447,15 @@ void loop() { out_speedr = constrain(set_speed * SPEED_COEFFICIENT_NRF - set_steer * STEER_COEFFICIENT_NRF, -1500, 1500); /* - Serial.print("Out steer="); - Serial.println(out_steer);*/ + 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) { - Serial.println("Armed but packet not ok"); + Serial1.println("Armed but packet not ok"); } #endif @@ -440,11 +466,13 @@ void loop() { if (controlmode==MODE_DISARMED) { //check if gametrak can be armed if (gt_length>gt_length_set && gt_length= 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 @@ -635,7 +741,7 @@ void ReceiveSerial2() memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback)); // Print data to built-in Serial - /*Serial.print("1: "); Serial.print(Feedback.cmd1); + 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); @@ -645,7 +751,7 @@ void ReceiveSerial2() //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"); @@ -655,4 +761,8 @@ void ReceiveSerial2() // Update previous states incomingBytePrev = incomingByte; -} \ No newline at end of file +} +*/ + + + diff --git a/hoverboard-firmware-hack-foc-serial-esc b/hoverboard-firmware-hack-foc-serial-esc index b9bf849..dc07f85 160000 --- a/hoverboard-firmware-hack-foc-serial-esc +++ b/hoverboard-firmware-hack-foc-serial-esc @@ -1 +1 @@ -Subproject commit b9bf849330f8e7355d0881300ebdc59607845e2d +Subproject commit dc07f8564ff92e0f57e0846a2980cc74b98256f4 diff --git a/hoverboard-firmware-hack-serial-esc b/hoverboard-firmware-hack-serial-esc deleted file mode 160000 index 9c84e97..0000000 --- a/hoverboard-firmware-hack-serial-esc +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 9c84e97a58f6027a3b8fdfdcfeaf64985e2327ff diff --git a/nippleremote b/nippleremote index 448db8a..fe9ff4f 160000 --- a/nippleremote +++ b/nippleremote @@ -1 +1 @@ -Subproject commit 448db8a6d8fd75c82bf27294876f45d2eb764f0d +Subproject commit fe9ff4f27f5730a39c09bd35fb009aa71b386a05