hoverbrett/controller.ino

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

//#define DEBUG

#define PIN_LED PC13

#define SENDPERIOD 20 //ms

#define IMUUPDATEPERIOD 10 //ms
long last_imuupdated=0;


#include <IMUGY85.h>
//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 

IMUGY85 imu;
double ax, ay, az, gx, gy, gz, roll, pitch, yaw, mx,my,mz,ma;
double setYaw=0;


//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 <SPI.h>
#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 };

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 50

//command variables
boolean motorenabled=false; //set by nrfdata.commands




long last_send=0;

int16_t out_steer=0; //between -1000 and 1000
int16_t out_speed=0;
uint8_t out_checksum=0; //0= disable motors, 255=reserved,  1<=checksum<255
#define NRFDATA_CENTER 127

boolean armed=false;



void setup() {

  Serial.begin(115200); //Debug and Program. A9=TX1,  A10=RX1  (3v3 level)

  Serial2.begin(19200); //control.  B10=TX3, B11=RX3 (Serial2 is Usart 3)

  
  pinMode(PIN_LED, OUTPUT);
  digitalWrite(PIN_LED,HIGH);


  Serial.println("Initializing nrf24");
  
  radio.begin();
  
  
  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();

  Serial.println("Initializing IMU");
  imu.init();

  Serial.println("Initialized");


}

void loop() {

  if (millis()-last_imuupdated>IMUUPDATEPERIOD){
    updateIMU();
    last_imuupdated=millis();
  }
  
  //NRF24
  nrf_delay=millis()-last_nrfreceive; //update nrf delay
  if ( radio.available() )
  {
    //Serial.println("radio available ...");
    bool done = false;
    while (!done)
    {
      digitalWrite(PIN_LED, !digitalRead(PIN_LED));
      done = radio.read( &lastnrfdata, sizeof(nrfdata) );

      //parse commands
      motorenabled = (lastnrfdata.commands & (1 << 0)); //check bit 0
      if (!motorenabled){ //disable motors?
        armed=false;
      }

      if (lastnrfdata.speed==NRFDATA_CENTER && lastnrfdata.speed==NRFDATA_CENTER){ //arm only when centered        
        armed=true; //arm at first received packet
      }      

      uint8_t calcchecksum=(uint8_t)((lastnrfdata.steer+3)*(lastnrfdata.speed+13));
      if (lastnrfdata.checksum!=calcchecksum){ //checksum not ok?
        armed=false; 
      }else{ //checksum ok
        last_nrfreceive=millis();
      }
      
      #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);
      #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 (armed && nrf_delay>=MAX_NRFDELAY){ //too long since last sucessful nrf receive
    armed=false;
  }
  if (armed){
    //out_speed=(int16_t)( (lastnrfdata.y-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX );
    //out_steer=(int16_t)( -(lastnrfdata.x-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX );
    out_speed=(int16_t)( ((int16_t)(lastnrfdata.speed)-NRFDATA_CENTER)*1000/127 ); //-1000 to 1000
    //out_steer=(int16_t)( ((int16_t)(lastnrfdata.steer)-NRFDATA_CENTER)*1000/127 );
    //Test Mag    
    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/2,0,800);
    yawdiff*=yawdiffsign; //redo sign
    
    out_steer=(int16_t)( yawdiff );
    /*
    Serial.print("Out steer=");
    Serial.println(out_steer);*/
    
  }else{ //took too long since last nrf data
    out_steer=0;
    out_speed=0;
  }
  

  

  if (millis()-last_send>SENDPERIOD){
    //calculate checksum
    out_checksum = ((uint8_t) ((uint8_t)out_steer)*((uint8_t)out_speed)); //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_steer, sizeof(out_steer));
    Serial2.write((uint8_t *) &out_speed, sizeof(out_speed));
    Serial2.write((uint8_t *) &out_checksum, sizeof(out_checksum));
    last_send=millis();


    #ifdef DEBUG
    Serial.print(" steer=");
    Serial.print(out_steer);
    Serial.print(" speed=");
    Serial.print(out_speed);
    Serial.print(" checksum=");
    Serial.print(out_checksum);
    
    Serial.println();
    #endif
  }

}


void updateIMU()
{
  //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
   */
}