hoverbrett/hoverbrettctrl/src/main.cpp

#include <Arduino.h>
#define SERIAL_BAUD         115200      // [-] Baud rate for built-in Serial (used for the Serial Monitor)

#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
//128 x 64 px
#define SCREEN_WIDTH 128
#define SCREEN_HEIGHT 64
#define SCREEN_ADDRESS 0x3C
#define OLED_RESET     -1 // Reset pin # (or -1 if sharing Arduino reset pin)
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
#define DISPLAYUPDATE_INTERVAL 200

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 FILTER_NRF_SET_SPEED 0.1 //higher value, faster response. depends on CONTROLUPDATEPERIOD
#define FILTER_NRF_SET_STEER 0.1

#define GT_LENGTH_MIN 200 //minimum length for stuff to start happen

#define GT_LENGTH_1_OFFSET -22.5
#define GT_LENGTH_1_SCALE 2.5
#define GT_LENGTH_2_OFFSET 563.6
#define GT_LENGTH_2_SCALE 0.45

#define GT_LENGTH_CROSSOVERADC ((GT_LENGTH_2_OFFSET-GT_LENGTH_1_OFFSET)/(GT_LENGTH_1_SCALE-GT_LENGTH_2_SCALE)) //crossover point from adc, where first and second lines cross
#define GT_LENGTH_CROSSOVER_FEATHER 76.0 //how much adc change in both directions should be smoothed when switching between first and second line

#define GT_LENGTH_MAXLENGTH 2000 //maximum length in [mm]. maximum string length is around 2m80
#define GT_LENGTH_ADC_MAXDIFF 127 //maximum adc value difference between A and B poti. Used to detect scratching poti. during length calibration was 57
int raw_length_maxdiff=0;
//TODO: implement error for poti maxdiff
uint16_t gt_length=0; //0=rolled up, 1unit = 1mm
/* calibration 20220410
lenght[mm], adc
0,9
100,52
200,86
300,124
400,165
500,212
600,286
700,376
800,520
900,746
1000,984
1100,1198
1200,1404
1300,1628
1400,1853
1500,2107
1600,2316
1700,2538
1800,2730
1900,2942
2000,3150
*/

#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 400 //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 -250 //[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
unsigned long nrf_delay = 0;
unsigned long last_nrfreceive_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 updateDisplay(unsigned long loopmillis);


void setup() {
  Serial.begin(SERIAL_BAUD); //Debug and Program
  esc.init();

  analogReadResolution(12);

  pinMode(PIN_GAMETRAK_LENGTH_A, INPUT_PULLUP);
  pinMode(PIN_GAMETRAK_LENGTH_B, INPUT_PULLUP);
  pinMode(PIN_GAMETRAK_VERTICAL, INPUT_PULLUP);
  pinMode(PIN_GAMETRAK_HORIZONTAL, INPUT_PULLUP);

  Wire.begin();
  // SSD1306_SWITCHCAPVCC = generate display voltage from 3.3V internally
  if(!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    for(;;); // Don't proceed, loop forever
  }

  // Show initial display buffer contents on the screen --
  // the library initializes this with an Adafruit splash screen.
  display.display();

  display.clearDisplay();
  display.setTextSize(1);
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(10, 0);
  display.println(F("Radio Init"));
  display.display();      // Show initial text

  radio.begin();


  Serial.println("RF24 set rate"); 
  radio.setDataRate( RF24_250KBPS ); //set to slow data rate. default was 1MBPS
  //radio.setDataRate( RF24_1MBPS ); 
  //Serial.println("set channel"); 

  radio.setChannel(NRF24CHANNEL); //0 to 124 (inclusive)

  //Serial.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

  //Serial.println("open pipe"); 
  radio.openWritingPipe(pipes[0]); //write on pipe 0
  radio.openReadingPipe(1, pipes[1]); //read on pipe 1

  Serial.println("start listening"); 
  radio.startListening();

  display.clearDisplay();
  display.setTextSize(2); // Draw 2X-scale text
  display.setTextColor(SSD1306_WHITE);
  display.setCursor(10, 0);
  display.println(F("Started"));
  display.display();      // Show initial text
  
  
}


void loop() {
  unsigned long loopmillis=millis();
  



  if (loopmillis - last_adcupdated > ADC_UPDATEPERIOD) { //update analog readings
      
    int raw_length_a=analogRead(PIN_GAMETRAK_LENGTH_A);
    int raw_length_b=analogRead(PIN_GAMETRAK_LENGTH_B);
    
    raw_length_maxdiff=max(raw_length_maxdiff,abs(raw_length_a-raw_length_b));

    int raw_length=(raw_length_a+raw_length_b)/2;

    
    uint16_t gt_length_1 = GT_LENGTH_1_OFFSET+raw_length*GT_LENGTH_1_SCALE;
    uint16_t gt_length_2 = GT_LENGTH_2_OFFSET+raw_length*GT_LENGTH_2_SCALE;

    double crossovermapping=constrain(((raw_length-GT_LENGTH_CROSSOVERADC)/GT_LENGTH_CROSSOVER_FEATHER )/2.0+0.5, 0.0,1.0); //0 for first, 1 for second
    

    gt_length = constrain( gt_length_1*(1-crossovermapping) + gt_length_2*crossovermapping , 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();


    /*
    Serial.print("gt_length=");
    Serial.print(gt_length);
    Serial.print(", gt_vertical=");
    Serial.print(gt_vertical);
    Serial.print(", gt_horizontal=");
    Serial.print(gt_horizontal);
    

    
    Serial.print(" pl=");
    Serial.print(raw_length_a);
    Serial.print(", ");
    Serial.print(raw_length_b);
    
    Serial.print(", pv=");
    Serial.print(analogRead(PIN_GAMETRAK_VERTICAL));
    Serial.print(", ph=");
    Serial.print(analogRead(PIN_GAMETRAK_HORIZONTAL));
    Serial.print(" Ldiff=");
    Serial.println(abs(raw_length_a-raw_length_b));
    */
    

   /*
   static int _rawlengtharray[40];
   static int _rawlapos=0;
   _rawlengtharray[_rawlapos++]=raw_length;
   _rawlapos%=40;
   
   int rawlengthfilter=0;
   for (int p=0;p<40;p++) {
     rawlengthfilter+=_rawlengtharray[p];
   }
   rawlengthfilter/=40;

    static int maxldiff=0;
    maxldiff=max(maxldiff,abs(raw_length_a-raw_length_b));
    Serial.print("");
    Serial.print(rawlengthfilter);
    Serial.print(" maxldiff=");
    Serial.println(maxldiff);*/
    
  }

  //NRF24
  nrf_delay = loopmillis - last_nrfreceive; //update nrf delay
  if ( radio.available() )
  {
    //Serial.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_delay=loopmillis-last_nrfreceive; //for display purpose
      last_nrfreceive = loopmillis;


      //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
    Serial.println("nrf_delay>=MAX_NRFDELAY, disarmed!");
    #endif
  }
  if (controlmode == MODE_RADIONRF) { //is armed in nrf mode

    
    if (lastpacketOK) { //if lastnrfdata is valid
      if (loopmillis - last_controlupdate > CONTROLUPDATEPERIOD) {
        last_controlupdate = loopmillis;

        //out_speed=(int16_t)( (lastnrfdata.y-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX );
        //out_steer=(int16_t)( -(lastnrfdata.x-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX );
        int16_t new_set_speed = (int16_t)( ((int16_t)(lastnrfdata.speed) - NRFDATA_CENTER) * 1000 / 127 ); //-1000 to 1000
        int16_t new_set_steer = (int16_t)( ((int16_t)(lastnrfdata.steer) - NRFDATA_CENTER) * 1000 / 127 );

        
        set_speed = set_speed*(1.0-FILTER_NRF_SET_SPEED) + new_set_speed*FILTER_NRF_SET_SPEED; //simple Filter
        set_steer = set_steer*(1.0-FILTER_NRF_SET_STEER) + new_set_steer*FILTER_NRF_SET_STEER;

        //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) {
      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_set+10) { //is in trackable length
      controlmode=MODE_GAMETRAK; //enable gametrak mode
      Serial.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
      //Serial.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;
      }
    }
    
    static float safetymultiplier=1.0; //value to reduce output speed if gametrack is pointing too far down or up (string might be behind vehicle)
    #define GT_SAFETY_THRESHOLD_H 120  //above which value (abs) safety slowdown should start
    #define GT_SAFETY_THRESHOLD_V 120
    #define SAFETYMULTIPLIER_UPDATE_INTERVAL 100
    #define SAFETYMULTIPLIER_CHANGE_TIME_DOWN 2.0 //Time how long it should take to go from 100% to 0% when in safety slowdown
    #define SAFETYMULTIPLIER_CHANGE_TIME_UP 1.0
    static unsigned long last_safetymultiplier_update=0;

    
    if (loopmillis-last_safetymultiplier_update > SAFETYMULTIPLIER_UPDATE_INTERVAL) {
      if (abs(gt_vertical)>GT_SAFETY_THRESHOLD_V || abs(gt_horizontal)>GT_SAFETY_THRESHOLD_H) {
        safetymultiplier-=1.0/(1000.0/SAFETYMULTIPLIER_UPDATE_INTERVAL)/SAFETYMULTIPLIER_CHANGE_TIME_DOWN;
      }else{
        safetymultiplier+=1.0/(1000.0/SAFETYMULTIPLIER_UPDATE_INTERVAL)/SAFETYMULTIPLIER_CHANGE_TIME_UP;
      }
      safetymultiplier=constrain(safetymultiplier,0.0,1.0);
      last_safetymultiplier_update=loopmillis;
    }

    //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)*safetymultiplier;
    _out_speedr = constrain(set_speed * SPEED_COEFFICIENT_GT -  set_steer * STEER_COEFFICIENT_GT, -1000, 1000)*safetymultiplier;
    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);
    set_speed=0; //reset filter
    set_steer=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 = loopmillis;
  
  
  #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
  }

  

  esc.update(loopmillis);
  updateDisplay(loopmillis);
}

void updateDisplay(unsigned long loopmillis)
{
  static unsigned long last_updatedisplay=0;
  if (loopmillis-last_updatedisplay>DISPLAYUPDATE_INTERVAL) {
    display.clearDisplay();
    display.setTextSize(1);
    display.setTextColor(SSD1306_WHITE);
    display.setCursor(1, 0);
    display.print(F("MODE=")); 

    switch(controlmode) {
      case MODE_DISARMED:
        display.println(F("DISARMED"));
        break;
      case MODE_RADIONRF:
        display.println(F("RADIONRF"));
        break;
      case MODE_GAMETRAK:
        display.println(F("GAMETRAK"));
        break;
      default:
        display.println(F("UNDEF"));
        break;
    }
    
    static float maxcurL=0;
    static float maxcurR=0;
    maxcurL=max(maxcurL,esc.getFiltered_curL());
    maxcurR=max(maxcurR,esc.getFiltered_curR());

    static float mincurL=0;
    static float mincurR=0;
    mincurL=min(mincurL,esc.getFiltered_curL());
    mincurR=min(mincurR,esc.getFiltered_curR());

    static float maxspdL=0;
    
    display.print(F("Bat=")); display.print(esc.getFeedback_batVoltage()); display.print(F(" Temp=")); display.println(esc.getFeedback_boardTemp());
    display.print(F("nrf_delay=")); display.print(last_nrfreceive_delay); display.print(F(" L=")); display.println(gt_length);
    display.print(F("maxdiff=")); display.println(raw_length_maxdiff);
    display.print(F("CMD=")); display.print(esc.getCmdL()); display.print(F(", ")); display.print(esc.getCmdR());
    //display.print(F("FBC=")); display.print(esc.getFeedback_cmd1()); display.print(F(", ")); display.print(esc.getFeedback_cmd2()); 
    display.print(F(" spd=")); display.print(esc.getFeedback_speedL_meas()); display.print(F(", ")); display.println(esc.getFeedback_speedR_meas());
    display.print(F("DC max=")); display.print(maxcurL,1); display.print(F("/"));  display.println(maxcurR,1); // display.print(F(" min=")); display.print(mincurL,1); display.print(F("/"));  display.println(mincurR,1);
    display.print(F("trip=")); display.print(esc.getTrip(),0); display.print(F(",")); display.print(esc.getCurrentConsumed(),3); display.println(F("Ah"));
    
   /*
    display.print(F("H=")); display.print(gt_horizontal); display.print(F(" V=")); display.println(gt_vertical);
    display.print(F("CMD=")); display.print(esc.getCmdL()); display.print(F(", ")); display.println(esc.getCmdR());*/
    
    display.display();      // Show initial text
    last_updatedisplay=loopmillis;
  }
}