From 0d33dda103e7b1aefeef62752d5ace66d21f9000 Mon Sep 17 00:00:00 2001
From: Fisch <interfisch@gmx.net>
Date: Thu, 24 Mar 2022 22:14:04 +0100
Subject: [PATCH] fix reading trackpoint too fast

---
 nippleremote_firmware/src/main.cpp | 35 ++++++++++++++++++++++++++++--
 1 file changed, 33 insertions(+), 2 deletions(-)

diff --git a/nippleremote_firmware/src/main.cpp b/nippleremote_firmware/src/main.cpp
index e8c0dae..559a0b1 100644
--- a/nippleremote_firmware/src/main.cpp
+++ b/nippleremote_firmware/src/main.cpp
@@ -60,7 +60,7 @@ struct nrfdata {
 };
 
 long last_sendNRF=0;
-#define NRFSEND_DELAY 20 //ms
+#define NRFSEND_DELAY 45 //ms
 
 #define PIN_TOUCH 5
 long last_touch=0;
@@ -76,6 +76,7 @@ boolean motorenabled=false;
 #define PIN_POWERON 7
 
 #define TRACKPOINT_MAX 70 //value for maximum stick movement
+#define TRACKPOINT_MAX_ERROR 90 //if value above this amount, error is triggered (nipple position stuck. maybe fixed by slowing down reading interval)
 float speedscale=0.0;
 float steerscale=0.0;
 
@@ -120,6 +121,8 @@ void sendRF(nrfdata senddata);
 long readVcc();
 void setup_updateSpeedmode();
 
+unsigned long last_testprint=0;
+
 void setup() {
   //Mouse.begin();
   
@@ -214,6 +217,11 @@ void loop() {
     Serial.print(", ");
     Serial.println(d.y);
     #endif
+
+    Serial.print("DataReport: ");
+    Serial.print(d.x);
+    Serial.print(", ");
+    Serial.println(d.y);
     
   
     nrfdata senddata;
@@ -226,14 +234,30 @@ void loop() {
     int16_t xin;
     if (d.x>=0 && d.x<=127){ //positive range
       xin=map(constrain((int16_t)d.x,0,TRACKPOINT_MAX) , 0,TRACKPOINT_MAX, 0, 1000 );
+      if ((float)d.x>TRACKPOINT_MAX_ERROR) { //value outside expected range
+        touching=false; //simulate hands off (needs recentering to reenable)
+        motorenabled=false;
+      }
     }else{ //negative range  128(=-1000) to 255(0)
       xin=map(constrain((int16_t)d.x,127+TRACKPOINT_MAX,255) , 127+TRACKPOINT_MAX,255, -1000, 0 );
+      if ((float)d.x<255-TRACKPOINT_MAX_ERROR) { //value outside expected range
+        touching=false; //simulate hands off (needs recentering to reenable)
+        motorenabled=false;
+      }
     }
     int16_t yin;
     if (d.y>=0 && d.y<=127){ //positive range
       yin=map(constrain((float)d.y,0,TRACKPOINT_MAX) , 0,TRACKPOINT_MAX, 0, 1000 );
+      if ((float)d.y>TRACKPOINT_MAX_ERROR) { //value outside expected range
+        touching=false; //simulate hands off (needs recentering to reenable)
+        motorenabled=false;
+      }
     }else{ //negative range  128(=-1000) to 255(0)
       yin=map(constrain((float)d.y,127+TRACKPOINT_MAX,255) , 127+TRACKPOINT_MAX,255, -1000, 0 );
+      if ((float)d.y<255-TRACKPOINT_MAX_ERROR) { //value outside expected range
+        touching=false; //simulate hands off (needs recentering to reenable)
+        motorenabled=false;
+      }
     }
 
     last_xin=xin; //save position values for other stuff than control
@@ -316,12 +340,14 @@ void loop() {
     }else{
       senddata.commands|= motorenabled << 0; //motorenabled is bit 0
     }
+    
     #ifdef DEBUG
     Serial.print(senddata.steer);
     Serial.print(", ");
     Serial.println(senddata.speed);
     #endif
     
+    
 
     senddata.checksum=(uint8_t)((senddata.steer+3)*(senddata.speed+13));
     sendRF(senddata);
@@ -337,9 +363,14 @@ void loop() {
   }
 
   
+  if(millis()-last_testprint >= 250){ //Test print
+    Serial.print(last_xin); Serial.print(", "); Serial.println(last_yin);
+    last_testprint=millis();
+  }
+  
   if(millis()-last_touch <= TOUCH_TIMEOUT){ //is touched
     if (!touching && setupmode!=SETUP_DONE) { //was false, is touching again (and not during setup_done wait)
-      Serial.println("touching was false");
+      //Serial.println("touching was false");
       if (last_xin==0 && last_yin==0) { //stick at center position
         touching=true; //enable only if stick is at center again
         motorenabled=true;