add gametrak reading

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_OFFSET 2048 //adc reading at 0A, with CJMCU-758 typically at Vcc/2. measured with actual voltage supply in hoverbrett
-#define CURRENT_FACTOR 0.4320376 //how much current (A) equals one adc unit
+#define CURRENT_FACTOR 0.38461538461538461538 //how much current (A) equals one adc unit.  2045-2032=13 at 5A
-double vbat=0; //battery voltage
+float vbat=0; //battery voltage
-double ibat=0; //battery current
+float ibat=0; //battery current
-long last_uiupdated=0;
+long last_adcupdated=0;
-#define UI_UPDATEPERIOD 10 //in ms
+#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
+#define PIN_GAMETRAK_LENGTH PA1  //yellow (connector) / orange (gametrak module wires): length
-long last_imuupdated = 0;
+#define PIN_GAMETRAK_VERTICAL PA3 //orange / red: vertical
-#define MAX_YAWCHANGE 90 //in degrees, if exceeded in one update intervall error will be triggered
+#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("gt_length=");
-    Serial.print(vbat);
+    Serial.print(gt_length);
-    Serial.print(", ibat=");
+    Serial.print(", gt_vertical=");
-    Serial.println(ibat);*/
+    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
+  } else { //disarmed
-  out_steer = 0;
+    out_steer = 0;
-  out_speed = 0;
+    out_speed = 0;
-  setYaw = yaw;
+    setYaw = yaw;
-  magalign_multiplier = 0;
+    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)
   }
 
-  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));
+  #ifdef PARAMETEROUTPUT
-  last_send = millis();
+  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.write((uint8_t *) &out_steer, sizeof(out_steer)); //int16_t, 2 bytes
-  Serial.print(" steer=");
+    Serial.write((uint8_t *) &out_speed, sizeof(out_speed)); //int16_t, 2 bytes
-  Serial.print(out_steer);
+    Serial.write((uint8_t *) &vbat, sizeof(vbat)); //float, 4 bytes
-  Serial.print(" speed=");
+    Serial.write((uint8_t *) &ibat, sizeof(ibat)); //float, 4 bytes
-  Serial.print(out_speed);
+    float yaw_float=yaw;
-  Serial.print(" checksum=");
+    Serial.write((uint8_t *) &yaw_float, sizeof(yaw_float)); //float, 4 bytes
-  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;