add gametrak reading
This commit is contained in:
parent
03b600d4c8
commit
b1ada895bc
218
controller.ino
218
controller.ino
|
|
@ -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();
|
||||
|
||||
#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
|
||||
|
||||
/*
|
||||
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
|
||||
*/
|
||||
|
||||
|
||||
#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
|
||||
}
|
||||
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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue