diff --git a/hoverbrettctrl/lib/hoverboard-esc-serial-comm b/hoverbrettctrl/lib/hoverboard-esc-serial-comm index 97630cf..2bb9f3d 160000 --- a/hoverbrettctrl/lib/hoverboard-esc-serial-comm +++ b/hoverbrettctrl/lib/hoverboard-esc-serial-comm @@ -1 +1 @@ -Subproject commit 97630cfcff7fa813ace7d1cfb7d4268a07e98750 +Subproject commit 2bb9f3d672d3f664dc3f77c3a4cbaecf33f3a915 diff --git a/hoverbrettctrl/platformio.ini b/hoverbrettctrl/platformio.ini index 92d578f..7befa2a 100644 --- a/hoverbrettctrl/platformio.ini +++ b/hoverbrettctrl/platformio.ini @@ -17,4 +17,8 @@ framework = arduino monitor_speed = 115200 build_flags = - -D USB_SERIAL_HID \ No newline at end of file + -D USB_SERIAL_HID + + +lib_deps = + https://github.com/maniacbug/RF24 \ No newline at end of file diff --git a/hoverbrettctrl/src/main.cpp b/hoverbrettctrl/src/main.cpp index a17a991..1c37394 100644 --- a/hoverbrettctrl/src/main.cpp +++ b/hoverbrettctrl/src/main.cpp @@ -1,15 +1,336 @@ #include +#define SERIAL_BAUD 115200 // [-] Baud rate for built-in Serial (used for the Serial Monitor) + +uint8_t error = 0; +#define IMU_NO_CHANGE 2 //IMU values did not change for too long #include "hoverboard-esc-serial-comm.h" ESCSerialComm esc(Serial2); +//Serial1 = TX1=1, RX1=0 +//Serial2 = TX2=10, RX2=9 +//Serial3 = TX3=8, RX3=7 + + +#define PIN_GAMETRAK_LENGTH_A A6 //A6=20 +#define PIN_GAMETRAK_LENGTH_B A7 //A7=21 +#define PIN_GAMETRAK_VERTICAL A8 //A8=22 +#define PIN_GAMETRAK_HORIZONTAL A9 //A9=23 + + +long last_adcupdated=0; +#define ADC_UPDATEPERIOD 10 //in ms + +#define CONTROLUPDATEPERIOD 10 +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 //(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 +#include "nRF24L01.h" +#include "RF24.h" + +RF24 radio(14, 15); //ce, cs +//SCK D13 (Pro mini), A5 (bluepill),13 (teensy32) +//Miso D12 (Pro mini), A6 (bluepill),12 (teensy32) +//Mosi D11 (Pro mini), A7 (bluepill),11 (teensy32) + +// 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 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(SERIAL_BAUD); //Debug and Program + esc.init(); + + + pinMode(PIN_GAMETRAK_LENGTH_A, INPUT); + pinMode(PIN_GAMETRAK_LENGTH_B, INPUT); + pinMode(PIN_GAMETRAK_VERTICAL, INPUT); + pinMode(PIN_GAMETRAK_HORIZONTAL, INPUT); + + 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(); + + } void loop() { unsigned long loopmillis=millis(); + + + + + if (millis() - last_adcupdated > ADC_UPDATEPERIOD) { //update analog readings + + gt_length = constrain(( analogRead(PIN_GAMETRAK_LENGTH_A))*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 + } + } + + + + 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 ); + + + //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 + int16_t _out_speedl,_out_speedr; + _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); + esc.setSpeed(_out_speedl,_out_speedr); + + } + }//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 + int16_t _out_speedl,_out_speedr; + _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); + esc.setSpeed(_out_speedl,_out_speedr); + } + + + if (error > 0) { //disarm if error occured + controlmode = MODE_DISARMED; //force disarmed + } + + if (controlmode == MODE_DISARMED){ //all disarmed + esc.setSpeed(0,0); + } + + + + if (esc.sendPending(loopmillis)) { + //calculate checksum + out_checksum = ((uint8_t) ((uint8_t)esc.getCmdL()) * ((uint8_t)esc.getCmdR())); //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 disabled + esc.setSpeed(0,0); + } + 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 + } + + + esc.update(loopmillis); } \ No newline at end of file