hoverbrett/controller.ino

517 lines
17 KiB
C++

//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
//PA2 may be defective on my bluepill
//#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_steer_p=1.0;
#define GT_SPEED_LIMIT 300 //maximum out_speed value +-
#define GT_STEER_LIMIT 200 //maximum out_steer value +-
#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
#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 <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 };
#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_steer = 0; //between -1000 and 1000
int16_t out_speed = 0;
int16_t lastsend_out_steer = 0; //last value transmitted to motor controller
int16_t lastsend_out_speed = 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;
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)
//Serial1 max 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() {
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
armed = true; //arm 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)); //check bit 0
if (!motorenabled) { //disable motors?
armed = false;
}
}
#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 (error > 0) { //disarm if error occured
armed = false;
}
if (armed && nrf_delay >= MAX_NRFDELAY) { //too long since last sucessful nrf receive
armed = false;
#ifdef DEBUG
Serial.println("nrf_delay>=MAX_NRFDELAY, disarmed!");
#endif
}
if (armed) { //is armed
armed_gt = false; //if remote used disable gametrak
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 );
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 );
//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 out_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
out_steer = out_steer * (1 - magalign_multiplier) + out_steer_mag * magalign_multiplier;
setYaw = setYaw * magalign_multiplier + yaw * (1 - magalign_multiplier); //if magalign_multiplier 0, setYaw equals current yaw
/*
Serial.print("Out steer=");
Serial.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");
}
#endif
}else{ //not armed, keep values reset
setYaw = yaw;
magalign_multiplier = 0;
}
if (!armed_gt) { //check if gametrak will be armed
if (gt_length>gt_length_set && gt_length<gt_length_set+10) { //is in trackable length
armed_gt=true;
}
}else if (armed_gt && !armed){ //gametrak control active and not remote active
//Gametrak Control Code
motorenabled=true;
if (gt_length<=GT_LENGTH_MIN){ //let go
armed_gt=false;
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){
_gt_length_diff=0; //threshold
}
out_speed = constrain((int16_t)(_gt_length_diff*gt_speed_p),-GT_SPEED_LIMIT,GT_SPEED_LIMIT);
out_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 (!armed && !armed_gt){ //all disarmed
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));
lastsend_out_steer = out_steer; //remember last transmittet values (for stat sending)
lastsend_out_speed = out_speed;
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
}
//
#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
Serial.write((uint8_t *) &out_steer, sizeof(out_steer)); //int16_t, 2 bytes
Serial.write((uint8_t *) &out_speed, sizeof(out_speed)); //int16_t, 2 bytes
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 *) &gt_length, sizeof(gt_length)); //uint16_t, 2 bytes
Serial.write((uint8_t *) &gt_horizontal, sizeof(gt_horizontal)); //int8_t, 1 byte
Serial.write((uint8_t *) &gt_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
*/
}