update all controllers
This commit is contained in:
parent
454c49946d
commit
7c4c4c693a
|
|
@ -1,696 +0,0 @@
|
||||||
//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
|
|
||||||
|
|
||||||
//Flashing the hoverbrett controller:
|
|
||||||
/*
|
|
||||||
* connect uart adapter to serial port cable (the one with more red heatshrink)
|
|
||||||
* (disconnect xt30 power connector)
|
|
||||||
* set jumper on usb uart adapter to output 5V
|
|
||||||
* hold boot0 button (black, the outermost) while powering up (or restarting with small button next to it)
|
|
||||||
* flash
|
|
||||||
*/
|
|
||||||
|
|
||||||
//PA2 may be defective on my bluepill
|
|
||||||
|
|
||||||
#define SERIAL_CONTROL_BAUD 38400 // [-] Baud rate for HoverSerial (used to communicate with the hoverboard)
|
|
||||||
#define SERIAL_BAUD 115200 // [-] Baud rate for built-in Serial (used for the Serial Monitor)
|
|
||||||
#define START_FRAME 0xAAAA // [-] Start frme definition for reliable serial communication
|
|
||||||
|
|
||||||
//#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_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 <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_speedl = 0; //between -1000 and 1000
|
|
||||||
int16_t out_speedr = 0;
|
|
||||||
int16_t lastsend_out_speedl = 0; //last value transmitted to motor controller
|
|
||||||
int16_t lastsend_out_speedr = 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
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
// Global variables
|
|
||||||
uint8_t idx = 0; // Index for new data pointer
|
|
||||||
uint16_t bufStartFrame; // Buffer Start Frame
|
|
||||||
byte *p; // Pointer declaration for the new received data
|
|
||||||
byte incomingByte;
|
|
||||||
byte incomingBytePrev;
|
|
||||||
|
|
||||||
typedef struct{
|
|
||||||
uint16_t start;
|
|
||||||
int16_t speedLeft;
|
|
||||||
int16_t speedRight;
|
|
||||||
uint16_t checksum;
|
|
||||||
} SerialCommand;
|
|
||||||
SerialCommand Command;
|
|
||||||
|
|
||||||
typedef struct{
|
|
||||||
uint16_t start;
|
|
||||||
int16_t cmd1;
|
|
||||||
int16_t cmd2;
|
|
||||||
int16_t speedL;
|
|
||||||
int16_t speedR;
|
|
||||||
int16_t speedL_meas;
|
|
||||||
int16_t speedR_meas;
|
|
||||||
int16_t batVoltage;
|
|
||||||
int16_t boardTemp;
|
|
||||||
int16_t curL_DC;
|
|
||||||
int16_t curR_DC;
|
|
||||||
uint16_t checksum;
|
|
||||||
} SerialFeedback;
|
|
||||||
SerialFeedback Feedback;
|
|
||||||
SerialFeedback NewFeedback;
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
void setup() {
|
|
||||||
|
|
||||||
Serial.begin(SERIAL_BAUD); //Debug and Program. A9=TX1, A10=RX1 (3v3 level)
|
|
||||||
|
|
||||||
Serial2.begin(SERIAL_CONTROL_BAUD); //control. B10=TX3, B11=RX3 (Serial2 is Usart 3)
|
|
||||||
//Serial1 may 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() {
|
|
||||||
|
|
||||||
ReceiveSerial2(); // Check for new received data
|
|
||||||
|
|
||||||
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
|
|
||||||
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
|
|
||||||
}
|
|
||||||
|
|
||||||
/*
|
|
||||||
#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 (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 (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 );
|
|
||||||
|
|
||||||
//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 set_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
|
|
||||||
|
|
||||||
set_steer = set_steer * (1 - magalign_multiplier) + set_steer_mag * magalign_multiplier;
|
|
||||||
*/
|
|
||||||
|
|
||||||
setYaw = setYaw * magalign_multiplier + yaw * (1 - magalign_multiplier); //if magalign_multiplier 0, setYaw equals current yaw
|
|
||||||
|
|
||||||
//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
|
|
||||||
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);
|
|
||||||
|
|
||||||
/*
|
|
||||||
Serial.print("Out steer=");
|
|
||||||
Serial.println(out_steer);*/
|
|
||||||
}
|
|
||||||
}//if pastpacket not ok, keep last out_steer and speed values until disarmed
|
|
||||||
|
|
||||||
if (!motorenabled) { //radio connected but not actively driving, keep values reset
|
|
||||||
setYaw = yaw;
|
|
||||||
magalign_multiplier = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
#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
|
|
||||||
}
|
|
||||||
}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
|
|
||||||
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
|
|
||||||
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);
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
if (error > 0) { //disarm if error occured
|
|
||||||
controlmode = MODE_DISARMED; //force disarmed
|
|
||||||
}
|
|
||||||
|
|
||||||
if (controlmode == MODE_DISARMED){ //all disarmed
|
|
||||||
out_speedl = 0;
|
|
||||||
out_speedr = 0;
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
if (millis() - last_send > SENDPERIOD) {
|
|
||||||
//calculate checksum
|
|
||||||
out_checksum = ((uint8_t) ((uint8_t)out_speedl) * ((uint8_t)out_speedr)); //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_speedl, sizeof(out_speedl));
|
|
||||||
Serial2.write((uint8_t *) &out_speedr, sizeof(out_speedr));
|
|
||||||
Serial2.write((uint8_t *) &out_checksum, sizeof(out_checksum));*/
|
|
||||||
if (motorenabled) { //motors enabled
|
|
||||||
SendSerial2(out_speedl,out_speedr);
|
|
||||||
} else { //motors disabled
|
|
||||||
SendSerial2(0,0);
|
|
||||||
}
|
|
||||||
lastsend_out_speedl = out_speedl; //remember last transmittet values (for stat sending)
|
|
||||||
lastsend_out_speedr = out_speedr;
|
|
||||||
last_send = millis();
|
|
||||||
|
|
||||||
|
|
||||||
#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
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
//
|
|
||||||
#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
|
|
||||||
|
|
||||||
uint8_t booleanvalues=0; //reset
|
|
||||||
booleanvalues |= motorenabled<<0; //bit 0
|
|
||||||
booleanvalues |= (controlmode&0b00000011)<<1; //bit 1 and 2 (2bit number for controlmodes (3)
|
|
||||||
|
|
||||||
Serial.write((uint8_t *) &out_speedl, sizeof(out_speedl)); //int16_t, 2 bytes
|
|
||||||
Serial.write((uint8_t *) &out_speedr, sizeof(out_speedr)); //int16_t, 2 bytes
|
|
||||||
Serial.write((uint8_t *) &booleanvalues, sizeof(booleanvalues)); //uint8_t, 1 byte //booleanvalues
|
|
||||||
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 *) >_length, sizeof(gt_length)); //uint16_t, 2 bytes
|
|
||||||
Serial.write((uint8_t *) >_horizontal, sizeof(gt_horizontal)); //int8_t, 1 byte
|
|
||||||
Serial.write((uint8_t *) >_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
|
|
||||||
*/
|
|
||||||
}
|
|
||||||
|
|
||||||
|
|
||||||
// ########################## SEND ##########################
|
|
||||||
void SendSerial2(int16_t uSpeedLeft, int16_t uSpeedRight)
|
|
||||||
{
|
|
||||||
// Create command
|
|
||||||
Command.start = (uint16_t)START_FRAME;
|
|
||||||
Command.speedLeft = (int16_t)uSpeedLeft;
|
|
||||||
Command.speedRight = (int16_t)uSpeedRight;
|
|
||||||
Command.checksum = (uint16_t)(Command.start ^ Command.speedLeft ^ Command.speedRight);
|
|
||||||
|
|
||||||
// Write to Serial
|
|
||||||
Serial2.write((uint8_t *) &Command, sizeof(Command));
|
|
||||||
}
|
|
||||||
|
|
||||||
// ########################## RECEIVE ##########################
|
|
||||||
void ReceiveSerial2()
|
|
||||||
{
|
|
||||||
// Check for new data availability in the Serial buffer
|
|
||||||
if (Serial2.available()) {
|
|
||||||
incomingByte = Serial2.read(); // Read the incoming byte
|
|
||||||
bufStartFrame = ((uint16_t)(incomingBytePrev) << 8) + incomingByte; // Construct the start frame
|
|
||||||
}
|
|
||||||
else {
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
// If DEBUG_RX is defined print all incoming bytes
|
|
||||||
#ifdef DEBUG_RX
|
|
||||||
Serial.print(incomingByte);
|
|
||||||
return;
|
|
||||||
#endif
|
|
||||||
|
|
||||||
// Copy received data
|
|
||||||
if (bufStartFrame == START_FRAME) { // Initialize if new data is detected
|
|
||||||
p = (byte *)&NewFeedback;
|
|
||||||
*p++ = incomingBytePrev;
|
|
||||||
*p++ = incomingByte;
|
|
||||||
idx = 2;
|
|
||||||
} else if (idx >= 2 && idx < sizeof(SerialFeedback)) { // Save the new received data
|
|
||||||
*p++ = incomingByte;
|
|
||||||
idx++;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Check if we reached the end of the package
|
|
||||||
if (idx == sizeof(SerialFeedback)) {
|
|
||||||
uint16_t checksum;
|
|
||||||
checksum = (uint16_t)(NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.cmd2 ^ NewFeedback.speedR ^ NewFeedback.speedL
|
|
||||||
^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp ^ NewFeedback.curL_DC ^ NewFeedback.curR_DC);
|
|
||||||
|
|
||||||
// Check validity of the new data
|
|
||||||
if (NewFeedback.start == START_FRAME && checksum == NewFeedback.checksum) {
|
|
||||||
// Copy the new data
|
|
||||||
memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback));
|
|
||||||
|
|
||||||
// Print data to built-in Serial
|
|
||||||
Serial.print("1: "); Serial.print(Feedback.cmd1);
|
|
||||||
Serial.print(" 2: "); Serial.print(Feedback.cmd2);
|
|
||||||
Serial.print(" 3: "); Serial.print(Feedback.speedR);
|
|
||||||
Serial.print(" 4: "); Serial.print(Feedback.speedL);
|
|
||||||
Serial.print(" 5: "); Serial.print(Feedback.speedR_meas);
|
|
||||||
Serial.print(" 6: "); Serial.print(Feedback.speedL_meas);
|
|
||||||
//Serial.print(" 7: "); Serial.print(Feedback.batVoltage);
|
|
||||||
//Serial.print(" 8: "); Serial.println(Feedback.boardTemp);
|
|
||||||
Serial.print(" 9: "); Serial.print(Feedback.curL_DC); //in A, in hoverbrett negative sign for forward
|
|
||||||
Serial.print(" 10: "); Serial.println(Feedback.curR_DC); //in A, in hoverbrett negative sign for forward
|
|
||||||
} else {
|
|
||||||
Serial.println("Non-valid data skipped");
|
|
||||||
}
|
|
||||||
idx = 0; // Reset the index (it prevents to enter in this if condition in the next cycle)
|
|
||||||
}
|
|
||||||
|
|
||||||
// Update previous states
|
|
||||||
incomingBytePrev = incomingByte;
|
|
||||||
}
|
|
||||||
|
|
@ -1 +1,6 @@
|
||||||
.pio
|
.pio
|
||||||
|
.vscode/.browse.c_cpp.db*
|
||||||
|
.vscode/c_cpp_properties.json
|
||||||
|
.vscode/launch.json
|
||||||
|
.vscode/ipch
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -1,142 +0,0 @@
|
||||||
{
|
|
||||||
"configurations": [
|
|
||||||
{
|
|
||||||
"name": "!!! WARNING !!! AUTO-GENERATED FILE, PLEASE DO NOT MODIFY IT AND USE https://docs.platformio.org/page/projectconf/section_env_build.html#build-flags"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"name": "Linux",
|
|
||||||
"includePath": [
|
|
||||||
"/media/fisch/HDD/Projects/hoverbrett/controller_pio/include",
|
|
||||||
"/media/fisch/HDD/Projects/hoverbrett/controller_pio/src",
|
|
||||||
"/media/fisch/HDD/Projects/hoverbrett/controller_pio/.pio/libdeps/genericSTM32F103C8/RF24_ID433",
|
|
||||||
"/media/fisch/HDD/Projects/hoverbrett/controller_pio/.pio/libdeps/genericSTM32F103C8/RF24_ID433/utility",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/SPI/src",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/cores/maple",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/system/libmaple",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/system/libmaple/include",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/system/libmaple/stm32f1/include",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/system/libmaple/usb/stm32f1",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/system/libmaple/usb/usb_lib",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/variants/generic_stm32f103c",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/A_STM32_Examples",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Adafruit_GFX_AS",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Adafruit_ILI9341",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Adafruit_ILI9341_STM",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Adafruit_SSD1306",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/EEPROM",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Ethernet_STM/src",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/FreeRTOS701",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/FreeRTOS701/utility",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/FreeRTOS821",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/FreeRTOS821/utility",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/FreeRTOS900",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Lcd7920_STM",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/LiquidCrystal",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/MapleCoOS",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/MapleCoOS/utility",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/MapleCoOS116",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/MapleCoOS116/utility",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/OLED_I2C",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/OneWireSTM/src",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/RTClock/src",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/SDIO",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/STM32ADC/src",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Serasidis_EtherCard_STM/src",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Serasidis_VS1003B_STM/src",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Serasidis_XPT2046_touch/src",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Servo/src",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Touch-Screen-Library_STM",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/USBComposite",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/WS2812B/src",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Wire",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Wire/utility",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/WireSlave/src",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/stm_fft",
|
|
||||||
"/home/fisch/.platformio/packages/tool-unity",
|
|
||||||
""
|
|
||||||
],
|
|
||||||
"browse": {
|
|
||||||
"limitSymbolsToIncludedHeaders": true,
|
|
||||||
"path": [
|
|
||||||
"/media/fisch/HDD/Projects/hoverbrett/controller_pio/include",
|
|
||||||
"/media/fisch/HDD/Projects/hoverbrett/controller_pio/src",
|
|
||||||
"/media/fisch/HDD/Projects/hoverbrett/controller_pio/.pio/libdeps/genericSTM32F103C8/RF24_ID433",
|
|
||||||
"/media/fisch/HDD/Projects/hoverbrett/controller_pio/.pio/libdeps/genericSTM32F103C8/RF24_ID433/utility",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/SPI/src",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/cores/maple",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/system/libmaple",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/system/libmaple/include",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/system/libmaple/stm32f1/include",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/system/libmaple/usb/stm32f1",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/system/libmaple/usb/usb_lib",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/variants/generic_stm32f103c",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/A_STM32_Examples",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Adafruit_GFX_AS",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Adafruit_ILI9341",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Adafruit_ILI9341_STM",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Adafruit_SSD1306",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/EEPROM",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Ethernet_STM/src",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/FreeRTOS701",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/FreeRTOS701/utility",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/FreeRTOS821",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/FreeRTOS821/utility",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/FreeRTOS900",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Lcd7920_STM",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/LiquidCrystal",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/MapleCoOS",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/MapleCoOS/utility",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/MapleCoOS116",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/MapleCoOS116/utility",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/OLED_I2C",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/OneWireSTM/src",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/RTClock/src",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/SDIO",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/STM32ADC/src",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Serasidis_EtherCard_STM/src",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Serasidis_VS1003B_STM/src",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Serasidis_XPT2046_touch/src",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Servo/src",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Touch-Screen-Library_STM",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/USBComposite",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/WS2812B/src",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Wire",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/Wire/utility",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/WireSlave/src",
|
|
||||||
"/home/fisch/.platformio/packages/framework-arduinoststm32-maple/STM32F1/libraries/stm_fft",
|
|
||||||
"/home/fisch/.platformio/packages/tool-unity",
|
|
||||||
""
|
|
||||||
]
|
|
||||||
},
|
|
||||||
"defines": [
|
|
||||||
"PLATFORMIO=40304",
|
|
||||||
"STM32F103xB",
|
|
||||||
"STM32F1",
|
|
||||||
"ARDUINO=10808",
|
|
||||||
"ARDUINO_ARCH_STM32",
|
|
||||||
"ARDUINO_ARCH_STM32F1",
|
|
||||||
"ARDUINO_GENERIC_STM32F103C",
|
|
||||||
"MCU_STM32F103C8",
|
|
||||||
"__STM32F1__",
|
|
||||||
"BOARD_generic_stm32f103c",
|
|
||||||
"F_CPU=72000000L",
|
|
||||||
"VECT_TAB_ADDR=0x8000000",
|
|
||||||
"ERROR_LED_PORT=GPIOC",
|
|
||||||
"ERROR_LED_PIN=13",
|
|
||||||
"CONFIG_MAPLE_MINI_NO_DISABLE_DEBUG=1",
|
|
||||||
""
|
|
||||||
],
|
|
||||||
"intelliSenseMode": "clang-x64",
|
|
||||||
"cStandard": "c11",
|
|
||||||
"cppStandard": "c++11",
|
|
||||||
"compilerPath": "/home/fisch/.platformio/packages/toolchain-gccarmnoneeabi@1.70201.0/bin/arm-none-eabi-gcc",
|
|
||||||
"compilerArgs": [
|
|
||||||
"-mcpu=cortex-m3",
|
|
||||||
"-mthumb",
|
|
||||||
"-march=armv7-m",
|
|
||||||
""
|
|
||||||
]
|
|
||||||
}
|
|
||||||
],
|
|
||||||
"version": 4
|
|
||||||
}
|
|
||||||
|
|
@ -3,5 +3,8 @@
|
||||||
// for the documentation about the extensions.json format
|
// for the documentation about the extensions.json format
|
||||||
"recommendations": [
|
"recommendations": [
|
||||||
"platformio.platformio-ide"
|
"platformio.platformio-ide"
|
||||||
|
],
|
||||||
|
"unwantedRecommendations": [
|
||||||
|
"ms-vscode.cpptools-extension-pack"
|
||||||
]
|
]
|
||||||
}
|
}
|
||||||
|
|
|
||||||
|
|
@ -1,34 +0,0 @@
|
||||||
// AUTOMATICALLY GENERATED FILE. PLEASE DO NOT MODIFY IT MANUALLY
|
|
||||||
|
|
||||||
// PIO Unified Debugger
|
|
||||||
//
|
|
||||||
// Documentation: https://docs.platformio.org/page/plus/debugging.html
|
|
||||||
// Configuration: https://docs.platformio.org/page/projectconf/section_env_debug.html
|
|
||||||
|
|
||||||
{
|
|
||||||
"version": "0.2.0",
|
|
||||||
"configurations": [
|
|
||||||
{
|
|
||||||
"type": "platformio-debug",
|
|
||||||
"request": "launch",
|
|
||||||
"name": "PIO Debug",
|
|
||||||
"executable": "/media/fisch/HDD/Projects/hoverbrett/controller_pio/.pio/build/genericSTM32F103C8/firmware.elf",
|
|
||||||
"toolchainBinDir": "/home/fisch/.platformio/packages/toolchain-gccarmnoneeabi@1.70201.0/bin",
|
|
||||||
"svdPath": "/home/fisch/.platformio/platforms/ststm32/misc/svd/STM32F103xx.svd",
|
|
||||||
"preLaunchTask": {
|
|
||||||
"type": "PlatformIO",
|
|
||||||
"task": "Pre-Debug"
|
|
||||||
},
|
|
||||||
"internalConsoleOptions": "openOnSessionStart"
|
|
||||||
},
|
|
||||||
{
|
|
||||||
"type": "platformio-debug",
|
|
||||||
"request": "launch",
|
|
||||||
"name": "PIO Debug (skip Pre-Debug)",
|
|
||||||
"executable": "/media/fisch/HDD/Projects/hoverbrett/controller_pio/.pio/build/genericSTM32F103C8/firmware.elf",
|
|
||||||
"toolchainBinDir": "/home/fisch/.platformio/packages/toolchain-gccarmnoneeabi@1.70201.0/bin",
|
|
||||||
"svdPath": "/home/fisch/.platformio/platforms/ststm32/misc/svd/STM32F103xx.svd",
|
|
||||||
"internalConsoleOptions": "openOnSessionStart"
|
|
||||||
}
|
|
||||||
]
|
|
||||||
}
|
|
||||||
|
|
@ -8,6 +8,8 @@
|
||||||
; Please visit documentation for the other options and examples
|
; Please visit documentation for the other options and examples
|
||||||
; https://docs.platformio.org/page/projectconf.html
|
; https://docs.platformio.org/page/projectconf.html
|
||||||
|
|
||||||
|
;RF24@>=1.3.4 <- vorher benutzt
|
||||||
|
|
||||||
[env:genericSTM32F103C8]
|
[env:genericSTM32F103C8]
|
||||||
platform = ststm32
|
platform = ststm32
|
||||||
board = genericSTM32F103C8
|
board = genericSTM32F103C8
|
||||||
|
|
@ -17,5 +19,12 @@ upload_protocol = serial
|
||||||
|
|
||||||
monitor_speed = 115200
|
monitor_speed = 115200
|
||||||
|
|
||||||
|
build_flags =
|
||||||
|
-D HAVE_HWSERIAL1
|
||||||
|
-D HAVE_HWSERIAL3
|
||||||
|
|
||||||
lib_deps =
|
lib_deps =
|
||||||
RF24@>=1.3.4
|
https://github.com/maniacbug/RF24
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -1,8 +1,7 @@
|
||||||
#include <Arduino.h>
|
#include <Arduino.h>
|
||||||
|
|
||||||
void ReceiveSerial2();
|
|
||||||
void SendSerial2(int16_t uSpeedLeft, int16_t uSpeedRight);
|
|
||||||
|
|
||||||
|
//Arduino IDE Settings:
|
||||||
//https://github.com/rogerclarkmelbourne/Arduino_STM32 in arduino/hardware
|
//https://github.com/rogerclarkmelbourne/Arduino_STM32 in arduino/hardware
|
||||||
//Board: Generic STM32F103C series
|
//Board: Generic STM32F103C series
|
||||||
//Upload method: serial
|
//Upload method: serial
|
||||||
|
|
@ -12,7 +11,7 @@ void SendSerial2(int16_t uSpeedLeft, int16_t uSpeedRight);
|
||||||
//to flash set boot0 (the one further away from reset button) to 1 and press reset, flash, program executes immediately
|
//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
|
//set boot0 back to 0 to run program on powerup
|
||||||
|
|
||||||
//Flashing the hoverbrett controller:
|
//Flashing the hoverbrett controller (bluepill):
|
||||||
/*
|
/*
|
||||||
* connect uart adapter to serial port cable (the one with more red heatshrink)
|
* connect uart adapter to serial port cable (the one with more red heatshrink)
|
||||||
* (disconnect xt30 power connector)
|
* (disconnect xt30 power connector)
|
||||||
|
|
@ -23,12 +22,54 @@ void SendSerial2(int16_t uSpeedLeft, int16_t uSpeedRight);
|
||||||
|
|
||||||
//PA2 may be defective on my bluepill
|
//PA2 may be defective on my bluepill
|
||||||
|
|
||||||
#define SERIAL_CONTROL_BAUD 38400 // [-] Baud rate for HoverSerial (used to communicate with the hoverboard)
|
#define SERIAL_CONTROL_BAUD 115200 // [-] Baud rate for HoverSerial (used to communicate with the hoverboard)
|
||||||
#define SERIAL_BAUD 115200 // [-] Baud rate for built-in Serial (used for the Serial Monitor)
|
#define SERIAL_BAUD 115200 // [-] Baud rate for built-in Serial (used for the Serial Monitor)
|
||||||
#define START_FRAME 0xAAAA // [-] Start frme definition for reliable serial communication
|
#define START_FRAME 0xABCD // [-] Start frme definition for reliable serial communication
|
||||||
|
|
||||||
//#define DEBUG
|
//#define DEBUG
|
||||||
//#define PARAMETEROUTPUT
|
//#define PARAMETEROUTPUT
|
||||||
|
|
||||||
|
//void ReceiveSerial2();
|
||||||
|
//void SendSerial2(int16_t uSpeedLeft, int16_t uSpeedRight);
|
||||||
|
|
||||||
|
// Structs for serial communication
|
||||||
|
typedef struct{
|
||||||
|
uint8_t idx = 0; // Index for new data pointer
|
||||||
|
uint16_t bufStartFrame; // Buffer Start Frame
|
||||||
|
byte *p; // Pointer declaration for the new received data
|
||||||
|
byte incomingByte;
|
||||||
|
byte incomingBytePrev;
|
||||||
|
long lastValidDataSerial_time;
|
||||||
|
} SerialRead;
|
||||||
|
SerialRead Serialcom;
|
||||||
|
|
||||||
|
typedef struct{
|
||||||
|
uint16_t start;
|
||||||
|
int16_t speedLeft;
|
||||||
|
int16_t speedRight;
|
||||||
|
uint16_t checksum;
|
||||||
|
} SerialCommand;
|
||||||
|
SerialCommand Command;
|
||||||
|
|
||||||
|
typedef struct{ //match this struct to hoverboard-firmware SerialFeedback struct in main.c
|
||||||
|
uint16_t start;
|
||||||
|
int16_t cmd1;
|
||||||
|
int16_t cmd2;
|
||||||
|
int16_t speedL_meas; //left speed is positive when driving forward
|
||||||
|
int16_t speedR_meas; //right speed is negatie when driving forward
|
||||||
|
int16_t batVoltage;
|
||||||
|
int16_t boardTemp;
|
||||||
|
int16_t curL_DC; //negative values are current consumed. positive values mean generated current
|
||||||
|
int16_t curR_DC;
|
||||||
|
uint16_t cmdLed;
|
||||||
|
uint16_t checksum;
|
||||||
|
} SerialFeedback;
|
||||||
|
SerialFeedback FeedbackESC;
|
||||||
|
SerialFeedback NewFeedbackESC;
|
||||||
|
|
||||||
|
void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef);
|
||||||
|
bool ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &NewFeedback, HardwareSerial &SerialRef);
|
||||||
|
|
||||||
uint8_t error = 0;
|
uint8_t error = 0;
|
||||||
#define IMU_NO_CHANGE 2 //IMU values did not change for too long
|
#define IMU_NO_CHANGE 2 //IMU values did not change for too long
|
||||||
uint8_t imu_no_change_counter = 0;
|
uint8_t imu_no_change_counter = 0;
|
||||||
|
|
@ -63,7 +104,7 @@ long last_controlupdate = 0;
|
||||||
|
|
||||||
#define GT_LENGTH_OFFSET 4090 //adc offset value (rolled up value)
|
#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_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)
|
#define GT_LENGTH_SCALE -0.73 //(offset-adcvalue)*scale = length[mm] (+length_min)
|
||||||
//2720 at 1000mm+220mm -> 1370 for 1000mm ->
|
//2720 at 1000mm+220mm -> 1370 for 1000mm ->
|
||||||
#define GT_LENGTH_MAXLENGTH 2500 //maximum length in [mm]. maximum string length is around 2m80
|
#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
|
uint16_t gt_length=0; //0=rolled up, 1unit = 1mm
|
||||||
|
|
@ -179,48 +220,26 @@ uint8_t controlmode=0;
|
||||||
|
|
||||||
|
|
||||||
// Global variables
|
// Global variables
|
||||||
|
/*
|
||||||
uint8_t idx = 0; // Index for new data pointer
|
uint8_t idx = 0; // Index for new data pointer
|
||||||
uint16_t bufStartFrame; // Buffer Start Frame
|
uint16_t bufStartFrame; // Buffer Start Frame
|
||||||
byte *p; // Pointer declaration for the new received data
|
byte *p; // Pointer declaration for the new received data
|
||||||
byte incomingByte;
|
byte incomingByte;
|
||||||
byte incomingBytePrev;
|
byte incomingBytePrev;
|
||||||
|
*/
|
||||||
typedef struct{
|
|
||||||
uint16_t start;
|
|
||||||
int16_t speedLeft;
|
|
||||||
int16_t speedRight;
|
|
||||||
uint16_t checksum;
|
|
||||||
} SerialCommand;
|
|
||||||
SerialCommand Command;
|
|
||||||
|
|
||||||
typedef struct{
|
|
||||||
uint16_t start;
|
|
||||||
int16_t cmd1;
|
|
||||||
int16_t cmd2;
|
|
||||||
int16_t speedL;
|
|
||||||
int16_t speedR;
|
|
||||||
int16_t speedL_meas;
|
|
||||||
int16_t speedR_meas;
|
|
||||||
int16_t batVoltage;
|
|
||||||
int16_t boardTemp;
|
|
||||||
int16_t curL_DC;
|
|
||||||
int16_t curR_DC;
|
|
||||||
uint16_t checksum;
|
|
||||||
} SerialFeedback;
|
|
||||||
SerialFeedback Feedback;
|
|
||||||
SerialFeedback NewFeedback;
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
void setup() {
|
void setup() {
|
||||||
|
|
||||||
Serial.begin(SERIAL_BAUD); //Debug and Program. A9=TX1, A10=RX1 (3v3 level)
|
Serial1.begin(SERIAL_BAUD); //Debug and Program. A9=TX1, A10=RX1 (3v3 level)
|
||||||
|
|
||||||
|
|
||||||
Serial2.begin(SERIAL_CONTROL_BAUD); //control. B10=TX3, B11=RX3 (Serial2 is Usart 3)
|
Serial3.begin(SERIAL_CONTROL_BAUD); //control. B10=TX3, B11=RX3 (Serial3 is Usart 3)
|
||||||
//Serial1 may be dead on my board?
|
//Serial2 may be dead on my board?
|
||||||
|
|
||||||
|
analogReadResolution(12); //set resolution to 12 bit 0 - 4095
|
||||||
|
|
||||||
pinMode(PIN_LED, OUTPUT);
|
pinMode(PIN_LED, OUTPUT);
|
||||||
digitalWrite(PIN_LED, HIGH);
|
digitalWrite(PIN_LED, HIGH);
|
||||||
|
|
@ -235,32 +254,37 @@ void setup() {
|
||||||
|
|
||||||
|
|
||||||
#ifdef DEBUG
|
#ifdef DEBUG
|
||||||
Serial.println("Initializing nrf24");
|
Serial1.println("Initializing nrf24");
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
Serial1.println("radio begin");
|
||||||
radio.begin();
|
radio.begin();
|
||||||
|
|
||||||
|
|
||||||
|
//Serial1.println("set rate");
|
||||||
radio.setDataRate( RF24_250KBPS ); //set to slow data rate. default was 1MBPS
|
radio.setDataRate( RF24_250KBPS ); //set to slow data rate. default was 1MBPS
|
||||||
//radio.setDataRate( RF24_1MBPS );
|
//radio.setDataRate( RF24_1MBPS );
|
||||||
|
//Serial1.println("set channel");
|
||||||
|
|
||||||
radio.setChannel(NRF24CHANNEL); //0 to 124 (inclusive)
|
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.setRetries(15, 15); // optionally, increase the delay between retries & # of retries
|
||||||
radio.setPayloadSize(8); // optionally, reduce the payload size. seems to improve reliability
|
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.openWritingPipe(pipes[0]); //write on pipe 0
|
||||||
radio.openReadingPipe(1, pipes[1]); //read on pipe 1
|
radio.openReadingPipe(1, pipes[1]); //read on pipe 1
|
||||||
|
|
||||||
|
//Serial1.println("start listening");
|
||||||
radio.startListening();
|
radio.startListening();
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
#ifdef DEBUG
|
#ifdef DEBUG
|
||||||
Serial.println("Initialized");
|
Serial1.println("Initialized");
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
||||||
|
|
@ -269,7 +293,8 @@ void setup() {
|
||||||
void loop() {
|
void loop() {
|
||||||
|
|
||||||
|
|
||||||
ReceiveSerial2(); // Check for new received data
|
//ReceiveSerial2(); // Check for new received data
|
||||||
|
bool newData=ReceiveSerial(Serialcom,FeedbackESC, NewFeedbackESC, Serial3); // Check for new received data
|
||||||
|
|
||||||
|
|
||||||
if (millis() - last_adcupdated > ADC_UPDATEPERIOD) { //update analog readings
|
if (millis() - last_adcupdated > ADC_UPDATEPERIOD) { //update analog readings
|
||||||
|
|
@ -277,38 +302,39 @@ void loop() {
|
||||||
ibat=(analogRead(PIN_CURRENT)-CURRENT_OFFSET)*CURRENT_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);
|
gt_length = constrain(( analogRead(PIN_GAMETRAK_LENGTH))*GT_LENGTH_SCALE - (GT_LENGTH_SCALE*GT_LENGTH_OFFSET) +GT_LENGTH_MIN, 0,GT_LENGTH_MAXLENGTH);
|
||||||
if (gt_length<=GT_LENGTH_MIN){
|
if (gt_length<=GT_LENGTH_MIN){
|
||||||
gt_length=0; //if below minimum measurable length set to 0mm
|
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_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)-GT_HORIZONTAL_CENTER, +GT_HORIZONTAL_RANGE,-GT_HORIZONTAL_RANGE,-127,127),-127,127); //down 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();
|
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=");
|
Serial1.print("gt_length=");
|
||||||
Serial.print(analogRead(PIN_GAMETRAK_LENGTH));
|
Serial1.print(gt_length);
|
||||||
Serial.print(", PIN_GAMETRAK_VERTICAL=");
|
Serial1.print(", gt_vertical=");
|
||||||
Serial.print(analogRead(PIN_GAMETRAK_VERTICAL));
|
Serial1.print(gt_vertical);
|
||||||
Serial.print(", PIN_GAMETRAK_HORIZONTAL=");
|
Serial1.print(", gt_horizontal=");
|
||||||
Serial.println(analogRead(PIN_GAMETRAK_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
|
//NRF24
|
||||||
nrf_delay = millis() - last_nrfreceive; //update nrf delay
|
nrf_delay = millis() - last_nrfreceive; //update nrf delay
|
||||||
if ( radio.available() )
|
if ( radio.available() )
|
||||||
{
|
{
|
||||||
//Serial.println("radio available ...");
|
//Serial1.println("radio available ...");
|
||||||
|
|
||||||
lastpacketOK = false; //initialize with false, if checksum ok gets set to true
|
lastpacketOK = false; //initialize with false, if checksum ok gets set to true
|
||||||
digitalWrite(PIN_LED, !digitalRead(PIN_LED));
|
digitalWrite(PIN_LED, !digitalRead(PIN_LED));
|
||||||
|
|
@ -332,19 +358,19 @@ void loop() {
|
||||||
|
|
||||||
/*
|
/*
|
||||||
#ifdef DEBUG
|
#ifdef DEBUG
|
||||||
Serial.print("Received:");
|
Serial1.print("Received:");
|
||||||
Serial.print(" st=");
|
Serial1.print(" st=");
|
||||||
Serial.print(lastnrfdata.steer);
|
Serial1.print(lastnrfdata.steer);
|
||||||
Serial.print(", sp=");
|
Serial1.print(", sp=");
|
||||||
Serial.print(lastnrfdata.speed);
|
Serial1.print(lastnrfdata.speed);
|
||||||
Serial.print(", c=");
|
Serial1.print(", c=");
|
||||||
Serial.print(lastnrfdata.commands);
|
Serial1.print(lastnrfdata.commands);
|
||||||
Serial.print(", chks=");
|
Serial1.print(", chks=");
|
||||||
Serial.print(lastnrfdata.checksum);
|
Serial1.print(lastnrfdata.checksum);
|
||||||
|
|
||||||
Serial.print("nrfdelay=");
|
Serial1.print("nrfdelay=");
|
||||||
Serial.print(nrf_delay);
|
Serial1.print(nrf_delay);
|
||||||
Serial.println();
|
Serial1.println();
|
||||||
#endif
|
#endif
|
||||||
*/
|
*/
|
||||||
|
|
||||||
|
|
@ -361,10 +387,10 @@ void loop() {
|
||||||
}*/
|
}*/
|
||||||
|
|
||||||
/*
|
/*
|
||||||
Serial.print("setYaw=");
|
Serial1.print("setYaw=");
|
||||||
Serial.print(setYaw);
|
Serial1.print(setYaw);
|
||||||
Serial.print(" Yaw=");
|
Serial1.print(" Yaw=");
|
||||||
Serial.println(yaw);*/
|
Serial1.println(yaw);*/
|
||||||
|
|
||||||
|
|
||||||
}
|
}
|
||||||
|
|
@ -375,7 +401,7 @@ void loop() {
|
||||||
if (controlmode == MODE_RADIONRF && nrf_delay >= MAX_NRFDELAY) { //too long since last sucessful nrf receive
|
if (controlmode == MODE_RADIONRF && nrf_delay >= MAX_NRFDELAY) { //too long since last sucessful nrf receive
|
||||||
controlmode = MODE_DISARMED;
|
controlmode = MODE_DISARMED;
|
||||||
#ifdef DEBUG
|
#ifdef DEBUG
|
||||||
Serial.println("nrf_delay>=MAX_NRFDELAY, disarmed!");
|
Serial1.println("nrf_delay>=MAX_NRFDELAY, disarmed!");
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
if (controlmode == MODE_RADIONRF) { //is armed in nrf mode
|
if (controlmode == MODE_RADIONRF) { //is armed in nrf mode
|
||||||
|
|
@ -421,15 +447,15 @@ void loop() {
|
||||||
out_speedr = 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);
|
||||||
|
|
||||||
/*
|
/*
|
||||||
Serial.print("Out steer=");
|
Serial1.print("Out steer=");
|
||||||
Serial.println(out_steer);*/
|
Serial1.println(out_steer);*/
|
||||||
}
|
}
|
||||||
}//if pastpacket not ok, keep last out_steer and speed values until disarmed
|
}//if pastpacket not ok, keep last out_steer and speed values until disarmed
|
||||||
|
|
||||||
|
|
||||||
#ifdef DEBUG
|
#ifdef DEBUG
|
||||||
if (!lastpacketOK) {
|
if (!lastpacketOK) {
|
||||||
Serial.println("Armed but packet not ok");
|
Serial1.println("Armed but packet not ok");
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
@ -440,11 +466,13 @@ void loop() {
|
||||||
if (controlmode==MODE_DISARMED) { //check if gametrak can be armed
|
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
|
if (gt_length>gt_length_set && gt_length<gt_length_set+10) { //is in trackable length
|
||||||
controlmode=MODE_GAMETRAK; //enable gametrak mode
|
controlmode=MODE_GAMETRAK; //enable gametrak mode
|
||||||
|
Serial1.println("Enable Gametrak");
|
||||||
}
|
}
|
||||||
}else if (controlmode==MODE_GAMETRAK){ //gametrak control active and not remote active
|
}else if (controlmode==MODE_GAMETRAK){ //gametrak control active and not remote active
|
||||||
//Gametrak Control Code
|
//Gametrak Control Code
|
||||||
motorenabled=true;
|
motorenabled=true;
|
||||||
if (gt_length<=GT_LENGTH_MIN){ //let go
|
if (gt_length<=GT_LENGTH_MIN){ //let go
|
||||||
|
Serial1.println("gametrak released");
|
||||||
controlmode=MODE_DISARMED;
|
controlmode=MODE_DISARMED;
|
||||||
motorenabled=false;
|
motorenabled=false;
|
||||||
}
|
}
|
||||||
|
|
@ -498,13 +526,12 @@ void loop() {
|
||||||
out_checksum = 0; //checksum=0 disables motors
|
out_checksum = 0; //checksum=0 disables motors
|
||||||
}
|
}
|
||||||
|
|
||||||
/*Serial2.write((uint8_t *) &out_speedl, sizeof(out_speedl));
|
|
||||||
Serial2.write((uint8_t *) &out_speedr, sizeof(out_speedr));
|
|
||||||
Serial2.write((uint8_t *) &out_checksum, sizeof(out_checksum));*/
|
|
||||||
if (motorenabled) { //motors enabled
|
if (motorenabled) { //motors enabled
|
||||||
SendSerial2(out_speedl,out_speedr);
|
//SendSerial2(out_speedl,out_speedr);
|
||||||
|
SendSerial(Command,out_speedl,out_speedr,Serial3);
|
||||||
} else { //motors disabled
|
} else { //motors disabled
|
||||||
SendSerial2(0,0);
|
//SendSerial2(0,0);
|
||||||
|
SendSerial(Command,0,0,Serial3);
|
||||||
}
|
}
|
||||||
lastsend_out_speedl = out_speedl; //remember last transmittet values (for stat sending)
|
lastsend_out_speedl = out_speedl; //remember last transmittet values (for stat sending)
|
||||||
lastsend_out_speedr = out_speedr;
|
lastsend_out_speedr = out_speedr;
|
||||||
|
|
@ -512,17 +539,17 @@ void loop() {
|
||||||
|
|
||||||
|
|
||||||
#ifdef DEBUG
|
#ifdef DEBUG
|
||||||
Serial.print(" out_speedl=");
|
Serial1.print(" out_speedl=");
|
||||||
Serial.print(out_speedl);
|
Serial1.print(out_speedl);
|
||||||
Serial.print(" out_speedr=");
|
Serial1.print(" out_speedr=");
|
||||||
Serial.print(out_speedr);
|
Serial1.print(out_speedr);
|
||||||
Serial.print(" checksum=");
|
Serial1.print(" checksum=");
|
||||||
Serial.print(out_checksum);
|
Serial1.print(out_checksum);
|
||||||
|
|
||||||
Serial.print(" controlmode=");
|
Serial1.print(" controlmode=");
|
||||||
Serial.print(controlmode);
|
Serial1.print(controlmode);
|
||||||
|
|
||||||
Serial.println();
|
Serial1.println();
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
|
|
@ -564,14 +591,14 @@ void loop() {
|
||||||
/*
|
/*
|
||||||
void sendRF(nrfstatdata senddata){
|
void sendRF(nrfstatdata senddata){
|
||||||
#ifdef DEBUG
|
#ifdef DEBUG
|
||||||
Serial.println("Transmitting...");
|
Serial1.println("Transmitting...");
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
radio.stopListening(); //stop listening to be able to transmit
|
radio.stopListening(); //stop listening to be able to transmit
|
||||||
radiosendOk = radio.write( &senddata, sizeof(nrfstatdata) );
|
radiosendOk = radio.write( &senddata, sizeof(nrfstatdata) );
|
||||||
if (!radiosendOk){
|
if (!radiosendOk){
|
||||||
#ifdef DEBUG
|
#ifdef DEBUG
|
||||||
Serial.println("send failed");
|
Serial1.println("send failed");
|
||||||
#endif
|
#endif
|
||||||
}
|
}
|
||||||
radio.startListening(); //start listening again
|
radio.startListening(); //start listening again
|
||||||
|
|
@ -582,6 +609,17 @@ void sendRF(nrfstatdata senddata){
|
||||||
|
|
||||||
|
|
||||||
// ########################## SEND ##########################
|
// ########################## SEND ##########################
|
||||||
|
void SendSerial(SerialCommand &scom, int16_t uSpeedLeft, int16_t uSpeedRight, HardwareSerial &SerialRef)
|
||||||
|
{
|
||||||
|
// Create command
|
||||||
|
scom.start = (uint16_t)START_FRAME;
|
||||||
|
scom.speedLeft = (int16_t)uSpeedLeft;
|
||||||
|
scom.speedRight = (int16_t)uSpeedRight;
|
||||||
|
scom.checksum = (uint16_t)(scom.start ^ scom.speedLeft ^ scom.speedRight);
|
||||||
|
|
||||||
|
SerialRef.write((uint8_t *) &scom, sizeof(scom));
|
||||||
|
}
|
||||||
|
/*
|
||||||
void SendSerial2(int16_t uSpeedLeft, int16_t uSpeedRight)
|
void SendSerial2(int16_t uSpeedLeft, int16_t uSpeedRight)
|
||||||
{
|
{
|
||||||
// Create command
|
// Create command
|
||||||
|
|
@ -592,9 +630,77 @@ void SendSerial2(int16_t uSpeedLeft, int16_t uSpeedRight)
|
||||||
|
|
||||||
// Write to Serial
|
// Write to Serial
|
||||||
Serial2.write((uint8_t *) &Command, sizeof(Command));
|
Serial2.write((uint8_t *) &Command, sizeof(Command));
|
||||||
}
|
}*/
|
||||||
|
|
||||||
// ########################## RECEIVE ##########################
|
// ########################## RECEIVE ##########################
|
||||||
|
|
||||||
|
bool ReceiveSerial(SerialRead &sread, SerialFeedback &Feedback,SerialFeedback &NewFeedback, HardwareSerial &SerialRef)
|
||||||
|
{
|
||||||
|
bool _result=1;
|
||||||
|
// Check for new data availability in the Serial buffer
|
||||||
|
if ( SerialRef.available() ) {
|
||||||
|
sread.incomingByte = SerialRef.read(); // Read the incoming byte
|
||||||
|
sread.bufStartFrame = ((uint16_t)(sread.incomingByte) << 8) | sread.incomingBytePrev; // Construct the start frame
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
// If DEBUG_RX is defined print all incoming bytes
|
||||||
|
#ifdef DEBUG_RX
|
||||||
|
Serial.print(sread.incomingByte);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
// Copy received data
|
||||||
|
if (sread.bufStartFrame == START_FRAME) { // Initialize if new data is detected
|
||||||
|
sread.p = (byte *)&NewFeedback;
|
||||||
|
*sread.p++ = sread.incomingBytePrev;
|
||||||
|
*sread.p++ = sread.incomingByte;
|
||||||
|
sread.idx = 2;
|
||||||
|
} else if (sread.idx >= 2 && sread.idx < sizeof(SerialFeedback)) { // Save the new received data
|
||||||
|
*sread.p++ = sread.incomingByte;
|
||||||
|
sread.idx++;
|
||||||
|
}
|
||||||
|
|
||||||
|
// Check if we reached the end of the package
|
||||||
|
if (sread.idx == sizeof(SerialFeedback)) {
|
||||||
|
uint16_t checksum;
|
||||||
|
|
||||||
|
checksum = (uint16_t)(NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.cmd2
|
||||||
|
^ NewFeedback.speedR_meas ^ NewFeedback.speedL_meas ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp ^ NewFeedback.curL_DC ^ NewFeedback.curR_DC ^ NewFeedback.cmdLed);
|
||||||
|
|
||||||
|
// Check validity of the new data
|
||||||
|
if (NewFeedback.start == START_FRAME && checksum == NewFeedback.checksum) {
|
||||||
|
// Copy the new data
|
||||||
|
memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback));
|
||||||
|
sread.lastValidDataSerial_time = millis();
|
||||||
|
} else {
|
||||||
|
_result=0;
|
||||||
|
}
|
||||||
|
sread.idx = 0; // Reset the index (it prevents to enter in this if condition in the next cycle)
|
||||||
|
}
|
||||||
|
/*
|
||||||
|
// Print data to built-in Serial
|
||||||
|
Serial1.print("1: "); Serial.print(Feedback.cmd1);
|
||||||
|
Serial1.print(" 2: "); Serial.print(Feedback.cmd2);
|
||||||
|
Serial1.print(" 3: "); Serial.print(Feedback.speedR);
|
||||||
|
Serial1.print(" 4: "); Serial.print(Feedback.speedL);
|
||||||
|
Serial1.print(" 5: "); Serial.print(Feedback.speedR_meas);
|
||||||
|
Serial1.print(" 6: "); Serial.print(Feedback.speedL_meas);
|
||||||
|
Serial1.print(" 7: "); Serial.print(Feedback.batVoltage);
|
||||||
|
Serial1.print(" 8: "); Serial.println(Feedback.boardTemp);
|
||||||
|
} else {
|
||||||
|
Serial1.println("Non-valid data skipped");
|
||||||
|
}*/
|
||||||
|
|
||||||
|
// Update previous states
|
||||||
|
sread.incomingBytePrev = sread.incomingByte;
|
||||||
|
|
||||||
|
return _result; //new data was available
|
||||||
|
}
|
||||||
|
|
||||||
|
|
||||||
|
/*
|
||||||
void ReceiveSerial2()
|
void ReceiveSerial2()
|
||||||
{
|
{
|
||||||
// Check for new data availability in the Serial buffer
|
// Check for new data availability in the Serial buffer
|
||||||
|
|
@ -635,7 +741,7 @@ void ReceiveSerial2()
|
||||||
memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback));
|
memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback));
|
||||||
|
|
||||||
// Print data to built-in Serial
|
// Print data to built-in Serial
|
||||||
/*Serial.print("1: "); Serial.print(Feedback.cmd1);
|
Serial.print("1: "); Serial.print(Feedback.cmd1);
|
||||||
Serial.print(" 2: "); Serial.print(Feedback.cmd2);
|
Serial.print(" 2: "); Serial.print(Feedback.cmd2);
|
||||||
Serial.print(" 3: "); Serial.print(Feedback.speedR);
|
Serial.print(" 3: "); Serial.print(Feedback.speedR);
|
||||||
Serial.print(" 4: "); Serial.print(Feedback.speedL);
|
Serial.print(" 4: "); Serial.print(Feedback.speedL);
|
||||||
|
|
@ -645,7 +751,7 @@ void ReceiveSerial2()
|
||||||
//Serial.print(" 8: "); Serial.println(Feedback.boardTemp);
|
//Serial.print(" 8: "); Serial.println(Feedback.boardTemp);
|
||||||
Serial.print(" 9: "); Serial.print(Feedback.curL_DC); //in A, in hoverbrett negative sign for forward
|
Serial.print(" 9: "); Serial.print(Feedback.curL_DC); //in A, in hoverbrett negative sign for forward
|
||||||
Serial.print(" 10: "); Serial.println(Feedback.curR_DC); //in A, in hoverbrett negative sign for forward
|
Serial.print(" 10: "); Serial.println(Feedback.curR_DC); //in A, in hoverbrett negative sign for forward
|
||||||
*/
|
|
||||||
Serial.print(" 9: "); Serial.println(Feedback.curL_DC); //in A, in hoverbrett negative sign for forward
|
Serial.print(" 9: "); Serial.println(Feedback.curL_DC); //in A, in hoverbrett negative sign for forward
|
||||||
} else {
|
} else {
|
||||||
Serial.println("Non-valid data skipped");
|
Serial.println("Non-valid data skipped");
|
||||||
|
|
@ -656,3 +762,7 @@ void ReceiveSerial2()
|
||||||
// Update previous states
|
// Update previous states
|
||||||
incomingBytePrev = incomingByte;
|
incomingBytePrev = incomingByte;
|
||||||
}
|
}
|
||||||
|
*/
|
||||||
|
|
||||||
|
|
||||||
|
|
||||||
|
|
|
||||||
|
|
@ -1 +1 @@
|
||||||
Subproject commit b9bf849330f8e7355d0881300ebdc59607845e2d
|
Subproject commit dc07f8564ff92e0f57e0846a2980cc74b98256f4
|
||||||
|
|
@ -1 +0,0 @@
|
||||||
Subproject commit 9c84e97a58f6027a3b8fdfdcfeaf64985e2327ff
|
|
||||||
|
|
@ -1 +1 @@
|
||||||
Subproject commit 448db8a6d8fd75c82bf27294876f45d2eb764f0d
|
Subproject commit fe9ff4f27f5730a39c09bd35fb009aa71b386a05
|
||||||
Loading…
Reference in New Issue