fix reading trackpoint too fast

This commit is contained in:
interfisch 2022-03-24 22:14:04 +01:00
parent bc2ebcbebb
commit 0d33dda103
1 changed files with 33 additions and 2 deletions

View File

@ -60,7 +60,7 @@ struct nrfdata {
}; };
long last_sendNRF=0; long last_sendNRF=0;
#define NRFSEND_DELAY 20 //ms #define NRFSEND_DELAY 45 //ms
#define PIN_TOUCH 5 #define PIN_TOUCH 5
long last_touch=0; long last_touch=0;
@ -76,6 +76,7 @@ boolean motorenabled=false;
#define PIN_POWERON 7 #define PIN_POWERON 7
#define TRACKPOINT_MAX 70 //value for maximum stick movement #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 speedscale=0.0;
float steerscale=0.0; float steerscale=0.0;
@ -120,6 +121,8 @@ void sendRF(nrfdata senddata);
long readVcc(); long readVcc();
void setup_updateSpeedmode(); void setup_updateSpeedmode();
unsigned long last_testprint=0;
void setup() { void setup() {
//Mouse.begin(); //Mouse.begin();
@ -215,6 +218,11 @@ void loop() {
Serial.println(d.y); Serial.println(d.y);
#endif #endif
Serial.print("DataReport: ");
Serial.print(d.x);
Serial.print(", ");
Serial.println(d.y);
nrfdata senddata; nrfdata senddata;
@ -226,14 +234,30 @@ void loop() {
int16_t xin; int16_t xin;
if (d.x>=0 && d.x<=127){ //positive range if (d.x>=0 && d.x<=127){ //positive range
xin=map(constrain((int16_t)d.x,0,TRACKPOINT_MAX) , 0,TRACKPOINT_MAX, 0, 1000 ); 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) }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 ); 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; int16_t yin;
if (d.y>=0 && d.y<=127){ //positive range if (d.y>=0 && d.y<=127){ //positive range
yin=map(constrain((float)d.y,0,TRACKPOINT_MAX) , 0,TRACKPOINT_MAX, 0, 1000 ); 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) }else{ //negative range 128(=-1000) to 255(0)
yin=map(constrain((float)d.y,127+TRACKPOINT_MAX,255) , 127+TRACKPOINT_MAX,255, -1000, 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 last_xin=xin; //save position values for other stuff than control
@ -316,6 +340,7 @@ void loop() {
}else{ }else{
senddata.commands|= motorenabled << 0; //motorenabled is bit 0 senddata.commands|= motorenabled << 0; //motorenabled is bit 0
} }
#ifdef DEBUG #ifdef DEBUG
Serial.print(senddata.steer); Serial.print(senddata.steer);
Serial.print(", "); Serial.print(", ");
@ -323,6 +348,7 @@ void loop() {
#endif #endif
senddata.checksum=(uint8_t)((senddata.steer+3)*(senddata.speed+13)); senddata.checksum=(uint8_t)((senddata.steer+3)*(senddata.speed+13));
sendRF(senddata); 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(millis()-last_touch <= TOUCH_TIMEOUT){ //is touched
if (!touching && setupmode!=SETUP_DONE) { //was false, is touching again (and not during setup_done wait) 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 if (last_xin==0 && last_yin==0) { //stick at center position
touching=true; //enable only if stick is at center again touching=true; //enable only if stick is at center again
motorenabled=true; motorenabled=true;