copy all code from hoverbrett controller_pio

hoverbrettctrl/lib/hoverboard-esc-serial-comm

@@ -1 +1 @@
-Subproject commit 97630cfcff7fa813ace7d1cfb7d4268a07e98750
+Subproject commit 2bb9f3d672d3f664dc3f77c3a4cbaecf33f3a915

hoverbrettctrl/platformio.ini

@@ -18,3 +18,7 @@ monitor_speed = 115200
 
 build_flags = 
     -D USB_SERIAL_HID
+
+
+lib_deps =
+    https://github.com/maniacbug/RF24

hoverbrettctrl/src/main.cpp

@@ -1,15 +1,336 @@
 #include <Arduino.h>
+#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 <SPI.h>
+#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_set+10) { //is in trackable length
+      controlmode=MODE_GAMETRAK; //enable gametrak mode
+      Serial1.println("Enable Gametrak");
+    }
+  }else if (controlmode==MODE_GAMETRAK){ //gametrak control active and not remote active
+    //Gametrak Control Code
+    motorenabled=true;
+    if (gt_length<=GT_LENGTH_MIN){ //let go
+      Serial1.println("gametrak released");
+      controlmode=MODE_DISARMED;
+      motorenabled=false;
+    }
+    int16_t _gt_length_diff = gt_length-gt_length_set; //positive if needs to drive forward
+    if (_gt_length_diff>-GT_LENGTH_MINDIFF & _gt_length_diff<GT_LENGTH_MINDIFF){ //minimum difference to drive
+      _gt_length_diff=0; //threshold
+    }
+
+    set_steer=constrain((int16_t)(-gt_horizontal*gt_steer_p),-GT_STEER_LIMIT,GT_STEER_LIMIT); //steer positive is left //gt_horizontal left is negative
+    
+    if (_gt_length_diff>0) { //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);
 }