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 yin_smooth=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_WAIT 1 //waiting for input
@ -130,6 +133,7 @@ void setup() {
//Serial.print("CRC Length=");
//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.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;
//yin_smooth=smoothfilter*yin_smooth + (1-smoothfilter)*yin;
if (maxacc>0){
if (maxaccsteer>0){
// ### X ###
int16_t _xaccel=xin_smooth-xin;
if (_xaccel<-maxacc){ //limit acceleration
_xaccel=-maxacc;
}else if (_xaccel>maxacc){
_xaccel=maxacc;
if ((xin_smooth>0 && xin<-2) || (xin_smooth<0 && xin>2) ){ //if actively braking
if (_xaccel<-maxaccsteer_brake){ //limit deceleration
_xaccel=-maxaccsteer_brake;
}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
}else{ //no acc limit
xin_smooth=xin; //update immediately
}
if (maxacc>0){
// ### Y ###
int16_t _yaccel=yin_smooth-yin;
if ((yin_smooth>0 && yin<-2) || (yin_smooth<0 && yin>2) ){ //if actively braking
if (_yaccel<-maxacc_brake){ //limit deceleration
_yaccel=-maxacc_brake;
}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
}else{ //no acc limit
xin_smooth=xin; //update immediately
yin_smooth=yin;
}
@ -269,7 +295,7 @@ void loop() {
Serial.print(", ");
Serial.println(senddata.speed);
senddata.commands=0;
senddata.commands=0; //reset
if (!radioOk || setupmode!=SETUP_NONE){ //if last transmission failed or in setup mode
//senddata.steer=127; //stop
@ -344,12 +370,18 @@ void loop() {
setupmode_waitstarttime=millis();//use this value for done timer
}else if(last_xin < -SETUP_MOVE_THRESHOLD){ //y 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_waitstarttime=millis();//use this value for done timer
}else if(last_xin > SETUP_MOVE_THRESHOLD){ //y moved right
Serial.print("Moved Right");
maxacc=0;
maxacc_brake=0;
maxaccsteer=0;
maxaccsteer_brake=0;
setupmode=SETUP_DONE; //exit setupmode
setupmode_waitstarttime=millis();//use this value for done timer
}
@ -462,7 +494,7 @@ void setup_updateSpeedmode(){
break;
case 1: //medium
speedscale=0.6;
steerscale=0.4;
steerscale=0.45;
break;
case 2: //fast
speedscale=1.0;