slower nrf data rate and maxacc for braking and steering

This commit is contained in:
interfisch 2019-03-17 20:10:28 +01:00
parent c10be9a32d
commit 8d9b4fffdf
1 changed files with 46 additions and 14 deletions

View File

@ -82,6 +82,9 @@ int16_t last_yin=0;
int16_t xin_smooth=0; int16_t xin_smooth=0;
int16_t yin_smooth=0; int16_t yin_smooth=0;
int16_t maxacc=0; int16_t maxacc=0;
int16_t maxacc_brake=0;
int16_t maxaccsteer=0;
int16_t maxaccsteer_brake=0;
#define SETUP_NONE 0 #define SETUP_NONE 0
#define SETUP_WAIT 1 //waiting for input #define SETUP_WAIT 1 //waiting for input
@ -129,7 +132,8 @@ void setup() {
radio.begin(); radio.begin();
//Serial.print("CRC Length="); //Serial.print("CRC Length=");
//Serial.println(radio.getCRCLength()); //Serial.println(radio.getCRCLength());
radio.setDataRate( RF24_250KBPS ); //set to slow data rate. default was 1MBPS
radio.setRetries(15,15); // optionally, increase the delay between retries & # of retries radio.setRetries(15,15); // optionally, increase the delay between retries & # of retries
radio.setPayloadSize(8); // optionally, reduce the payload size. seems to improve reliability radio.setPayloadSize(8); // optionally, reduce the payload size. seems to improve reliability
@ -242,23 +246,45 @@ void loop() {
//xin_smooth=smoothfilter*xin_smooth + (1-smoothfilter)*xin; //xin_smooth=smoothfilter*xin_smooth + (1-smoothfilter)*xin;
//yin_smooth=smoothfilter*yin_smooth + (1-smoothfilter)*yin; //yin_smooth=smoothfilter*yin_smooth + (1-smoothfilter)*yin;
if (maxacc>0){ if (maxaccsteer>0){
// ### X ###
int16_t _xaccel=xin_smooth-xin; int16_t _xaccel=xin_smooth-xin;
if (_xaccel<-maxacc){ //limit acceleration if ((xin_smooth>0 && xin<-2) || (xin_smooth<0 && xin>2) ){ //if actively braking
_xaccel=-maxacc; if (_xaccel<-maxaccsteer_brake){ //limit deceleration
}else if (_xaccel>maxacc){ _xaccel=-maxaccsteer_brake;
_xaccel=maxacc; }else if (_xaccel>maxaccsteer_brake){
_xaccel=maxaccsteer_brake;
}
}else{ //not braking
if (_xaccel<-maxaccsteer){ //limit acceleration
_xaccel=-maxaccsteer;
}else if (_xaccel>maxaccsteer){
_xaccel=maxaccsteer;
}
} }
xin_smooth-=_xaccel; //update value xin_smooth-=_xaccel; //update value
}else{ //no acc limit
xin_smooth=xin; //update immediately
}
if (maxacc>0){
// ### Y ###
int16_t _yaccel=yin_smooth-yin; int16_t _yaccel=yin_smooth-yin;
if (_yaccel<-maxacc){ //limit acceleration if ((yin_smooth>0 && yin<-2) || (yin_smooth<0 && yin>2) ){ //if actively braking
_yaccel=-maxacc; if (_yaccel<-maxacc_brake){ //limit deceleration
}else if (_yaccel>maxacc){ _yaccel=-maxacc_brake;
_yaccel=maxacc; }else if (_yaccel>maxacc_brake){
_yaccel=maxacc_brake;
}
}else{ //not braking
if (_yaccel<-maxacc){ //limit acceleration
_yaccel=-maxacc;
}else if (_yaccel>maxacc){
_yaccel=maxacc;
}
} }
yin_smooth-=_yaccel; //update value yin_smooth-=_yaccel; //update value
}else{ //no acc limit }else{ //no acc limit
xin_smooth=xin; //update immediately
yin_smooth=yin; yin_smooth=yin;
} }
@ -269,7 +295,7 @@ void loop() {
Serial.print(", "); Serial.print(", ");
Serial.println(senddata.speed); Serial.println(senddata.speed);
senddata.commands=0; senddata.commands=0; //reset
if (!radioOk || setupmode!=SETUP_NONE){ //if last transmission failed or in setup mode if (!radioOk || setupmode!=SETUP_NONE){ //if last transmission failed or in setup mode
//senddata.steer=127; //stop //senddata.steer=127; //stop
@ -344,12 +370,18 @@ void loop() {
setupmode_waitstarttime=millis();//use this value for done timer setupmode_waitstarttime=millis();//use this value for done timer
}else if(last_xin < -SETUP_MOVE_THRESHOLD){ //y moved left }else if(last_xin < -SETUP_MOVE_THRESHOLD){ //y moved left
Serial.print("Moved Left"); Serial.print("Moved Left");
maxacc=50; maxacc=20; //the higher the snappier
maxacc_brake=30;
maxaccsteer=30;
maxaccsteer_brake=70;
setupmode=SETUP_DONE; //exit setupmode setupmode=SETUP_DONE; //exit setupmode
setupmode_waitstarttime=millis();//use this value for done timer setupmode_waitstarttime=millis();//use this value for done timer
}else if(last_xin > SETUP_MOVE_THRESHOLD){ //y moved right }else if(last_xin > SETUP_MOVE_THRESHOLD){ //y moved right
Serial.print("Moved Right"); Serial.print("Moved Right");
maxacc=0; maxacc=0;
maxacc_brake=0;
maxaccsteer=0;
maxaccsteer_brake=0;
setupmode=SETUP_DONE; //exit setupmode setupmode=SETUP_DONE; //exit setupmode
setupmode_waitstarttime=millis();//use this value for done timer setupmode_waitstarttime=millis();//use this value for done timer
} }
@ -462,7 +494,7 @@ void setup_updateSpeedmode(){
break; break;
case 1: //medium case 1: //medium
speedscale=0.6; speedscale=0.6;
steerscale=0.4; steerscale=0.45;
break; break;
case 2: //fast case 2: //fast
speedscale=1.0; speedscale=1.0;