interfisch
/
hoverbrett
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

add gametrak reading

This commit is contained in:
interfisch 2019-06-11 14:07:54 +02:00
parent 03b600d4c8
commit b1ada895bc
1 changed files with 169 additions and 55 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

224
controller.ino
View File

@ -7,7 +7,10 @@
//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;
@ -15,24 +18,49 @@ 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.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 2034 //adc reading at 0A, with CJMCU-758 typically at Vcc/2
#define CURRENT_FACTOR 0.4320376 //how much current (A) equals one adc unit
double vbat=0; //battery voltage
double ibat=0; //battery current
long last_uiupdated=0;
#define UI_UPDATEPERIOD 10 //in ms
#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
#define SENDPERIOD 20 //ms. delay for sending speed and steer data to motor controller via serial
//Status information sending
#define PARAMETERSENDPERIOD 200 //delay for sending stat data via nrf24
long last_parametersend=0;
#define CONTROLUPDATEPERIOD 10
long last_controlupdate = 0;
#define IMUUPDATEPERIOD 10 //ms
long last_imuupdated = 0;
#define MAX_YAWCHANGE 90 //in degrees, if exceeded in one update intervall error will be triggered
#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
#include <IMUGY85.h>
@ -41,6 +69,10 @@ long last_imuupdated = 0;
//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;
@ -48,7 +80,7 @@ double old_ax, old_ay, old_az, old_gx, old_gy, old_gz, old_roll, old_pitch, old_
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
@ -86,6 +118,9 @@ 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
@ -96,6 +131,8 @@ 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
@ -106,9 +143,10 @@ boolean lastpacketOK = false;
void setup() {
Serial.begin(115200); //Debug and Program. A9=TX1, A10=RX1 (3v3 level)
Serial.begin(57600); //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);
@ -118,12 +156,18 @@ void setup() {
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)
@ -135,10 +179,14 @@ void setup() {
radio.startListening();
#ifdef DEBUG
Serial.println("Initializing IMU");
#endif
imu.init();
Serial.println("Initialized");
#ifdef DEBUG
Serial.println("Initialized");
#endif
}
@ -150,15 +198,36 @@ void loop() {
last_imuupdated = millis();
}
if (millis() - last_uiupdated > UI_UPDATEPERIOD) { //update current and voltage
if (millis() - last_adcupdated > ADC_UPDATEPERIOD) { //update analog readings
vbat=analogRead(PIN_VBAT)*VBAT_DIV_FACTOR;
ibat=(analogRead(PIN_CURRENT)-CURRENT_OFFSET)*CURRENT_FACTOR;
last_uiupdated = millis();
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("vbat=");
Serial.print(vbat);
Serial.print(", ibat=");
Serial.println(ibat);*/
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
@ -180,7 +249,6 @@ void loop() {
uint8_t calcchecksum = (uint8_t)((lastnrfdata.steer + 3) * (lastnrfdata.speed + 13));
if (lastnrfdata.checksum == calcchecksum) { //checksum ok?
lastpacketOK = true;
@ -230,6 +298,7 @@ void loop() {
}
}
if (error > 0) { //disarm if error occured
armed = false;
}
@ -282,50 +351,93 @@ void loop() {
#ifdef DEBUG
if (!lastpacketOK)
Serial.println("Armed but packet not ok");
}
}
#endif
} else { //disarmed
out_steer = 0;
out_speed = 0;
setYaw = yaw;
magalign_multiplier = 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)
} else { //disarmed
out_steer = 0;
out_speed = 0;
setYaw = yaw;
magalign_multiplier = 0;
}
if (!motorenabled) { //disable motors?
out_checksum = 0; //checksum=0 disables motors
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
}
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 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
#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
}
/*
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
*/
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()
{
@ -333,7 +445,9 @@ void updateIMU()
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;