fix safety off
This commit is contained in:
parent
aea62ec868
commit
955696d428
|
|
@ -4,6 +4,8 @@
|
|||
unsigned long loopmillis;
|
||||
unsigned long last_loopmillis;
|
||||
|
||||
//for testing debug
|
||||
uint8_t debug_count_disarmedbecauseofnrfdelay=0;
|
||||
|
||||
#include <TimeLib.h> //for teensy rtc
|
||||
time_t getTeensy3Time();
|
||||
|
|
@ -65,7 +67,7 @@ bool button_down_click=false;
|
|||
#define PIN_GAMETRAK_HORIZONTAL A9 //A9=23
|
||||
|
||||
|
||||
float speed_coefficient_nrf=1.0; // higher value == stronger
|
||||
float speed_coefficient_nrf=0.4; // higher value == stronger
|
||||
float steer_coefficient_nrf=0.5; // higher value == stronger
|
||||
|
||||
float speed_coefficient_gt=1.0; // higher value == stronger
|
||||
|
|
@ -77,8 +79,7 @@ long last_adcupdated=0;
|
|||
#define ADC_UPDATEPERIOD 10 //in ms
|
||||
|
||||
#define CONTROLUPDATEPERIOD 10
|
||||
long last_controlupdate = 0;
|
||||
float filter_nrf_set_speed=0.02; //higher value, faster response. depends on CONTROLUPDATEPERIOD
|
||||
float filter_nrf_set_speed=0.06; //higher value, faster response. depends on CONTROLUPDATEPERIOD
|
||||
float filter_nrf_set_steer=0.06;
|
||||
|
||||
float filter_stop_set_speed=0.015; //safety stop
|
||||
|
|
@ -185,7 +186,7 @@ boolean setup_directon_press_right=false;
|
|||
|
||||
|
||||
|
||||
long last_send = 0;
|
||||
|
||||
|
||||
int16_t set_speed = 0;
|
||||
int16_t set_steer = 0;
|
||||
|
|
@ -462,11 +463,10 @@ void loop() {
|
|||
}else if ((lastnrfdata.commands & (1 << 2))>>0) { //up
|
||||
setup_directon_press_up = true;
|
||||
Serial.println("RF Button Up");
|
||||
speed_coefficient_nrf=1.0;
|
||||
}else if ((lastnrfdata.commands & (1 << 3))>>0) { //down
|
||||
setup_directon_press_down = true;
|
||||
Serial.println("RF Button Down");
|
||||
speed_coefficient_nrf=0.5;
|
||||
|
||||
}else if ((lastnrfdata.commands & (1 << 4))>>0) { //right
|
||||
setup_directon_press_right = true;
|
||||
Serial.println("RF Button Right");
|
||||
|
|
@ -477,17 +477,26 @@ void loop() {
|
|||
|
||||
|
||||
if (abs(set_speed)<10 && abs(set_steer)<10) { //standstill
|
||||
if (setup_directon_press_left) {
|
||||
|
||||
if (setup_directon_press_left || setup_directon_press_right) {
|
||||
//Fast riding mode
|
||||
speed_coefficient_nrf=1.0;
|
||||
steer_coefficient_nrf=0.3;
|
||||
filter_nrf_set_speed=0.015;
|
||||
filter_nrf_set_steer=0.08;
|
||||
}
|
||||
if (setup_directon_press_up) {
|
||||
|
||||
//Remote controlled Aggro
|
||||
speed_coefficient_nrf=1.0;
|
||||
steer_coefficient_nrf=0.5;
|
||||
filter_nrf_set_speed=0.08;
|
||||
filter_nrf_set_steer=0.08;
|
||||
}
|
||||
if (setup_directon_press_down) {
|
||||
|
||||
}
|
||||
if (setup_directon_press_right) {
|
||||
|
||||
//Remote controlled Walking
|
||||
speed_coefficient_nrf=0.4;
|
||||
steer_coefficient_nrf=0.5;
|
||||
filter_nrf_set_speed=0.06;
|
||||
filter_nrf_set_steer=0.06;
|
||||
}
|
||||
}
|
||||
setup_directon_press_left=false;
|
||||
|
|
@ -499,6 +508,7 @@ void loop() {
|
|||
|
||||
if (controlmode == MODE_RADIONRF && nrf_delay >= MAX_NRFDELAY) { //too long since last sucessful nrf receive
|
||||
controlmode = MODE_DISARMED;
|
||||
debug_count_disarmedbecauseofnrfdelay++;
|
||||
#ifdef DEBUG
|
||||
Serial.println("nrf_delay>=MAX_NRFDELAY, disarmed!");
|
||||
#endif
|
||||
|
|
@ -507,18 +517,28 @@ void loop() {
|
|||
|
||||
|
||||
if (lastpacketOK) { //if lastnrfdata is valid
|
||||
if (loopmillis - last_controlupdate > CONTROLUPDATEPERIOD) {
|
||||
last_controlupdate = loopmillis;
|
||||
static unsigned long last_controlupdate_validnrf=0;
|
||||
if (loopmillis - last_controlupdate_validnrf > CONTROLUPDATEPERIOD) {
|
||||
last_controlupdate_validnrf = loopmillis;
|
||||
|
||||
//out_speed=(int16_t)( (lastnrfdata.y-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX );
|
||||
//out_steer=(int16_t)( -(lastnrfdata.x-TRACKPOINT_CENTER)*1000/TRACKPOINT_MAX );
|
||||
int16_t new_set_speed = (int16_t)( ((int16_t)(lastnrfdata.speed) - NRFDATA_CENTER) * 1000 / 127 ); //-1000 to 1000
|
||||
int16_t new_set_steer = (int16_t)( ((int16_t)(lastnrfdata.steer) - NRFDATA_CENTER) * 1000 / 127 ) * -1;
|
||||
|
||||
if (!motorenabled) { //when safety off, reason: remote relased
|
||||
if (abs(new_set_speed) < abs(set_speed)) { //only allow going slower
|
||||
set_speed = set_speed*(1.0-filter_nrf_set_speed) + new_set_speed*filter_nrf_set_speed; //simple Filter
|
||||
}
|
||||
if (abs(new_set_steer) < abs(set_steer)) { //only allow turning slower
|
||||
set_steer = set_steer*(1.0-filter_nrf_set_steer) + new_set_steer*filter_nrf_set_steer;
|
||||
}
|
||||
|
||||
}else{
|
||||
set_speed = set_speed*(1.0-filter_nrf_set_speed) + new_set_speed*filter_nrf_set_speed; //simple Filter
|
||||
set_steer = set_steer*(1.0-filter_nrf_set_steer) + new_set_steer*filter_nrf_set_steer;
|
||||
|
||||
|
||||
//calculate speed l and r from speed and steer
|
||||
|
||||
int16_t _out_speedl,_out_speedr;
|
||||
|
|
@ -526,6 +546,7 @@ void loop() {
|
|||
_out_speedr = constrain(set_speed * speed_coefficient_nrf - set_steer * speed_coefficient_nrf * steer_coefficient_nrf, -1500, 1500);
|
||||
esc.setSpeed(_out_speedl,_out_speedr);
|
||||
}
|
||||
}
|
||||
}//if pastpacket not ok, keep last out_steer and speed values until disarmed
|
||||
|
||||
|
||||
|
|
@ -590,8 +611,9 @@ void loop() {
|
|||
}
|
||||
|
||||
//calculate speed l and r from speed and steer
|
||||
if (loopmillis - last_controlupdate > CONTROLUPDATEPERIOD) {
|
||||
last_controlupdate = loopmillis;
|
||||
static unsigned long last_controlupdate_gt=0;
|
||||
if (loopmillis - last_controlupdate_gt > CONTROLUPDATEPERIOD) {
|
||||
last_controlupdate_gt = loopmillis;
|
||||
int16_t _out_speedl,_out_speedr;
|
||||
_out_speedl = constrain(set_speed * speed_coefficient_gt + set_steer * speed_coefficient_gt * steer_coefficient_gt, -1000, 1000)*safetymultiplier;
|
||||
_out_speedr = constrain(set_speed * speed_coefficient_gt - set_steer * speed_coefficient_gt * steer_coefficient_gt, -1000, 1000)*safetymultiplier;
|
||||
|
|
@ -611,8 +633,9 @@ void loop() {
|
|||
set_speed=0;
|
||||
set_steer=0;
|
||||
}
|
||||
if (loopmillis - last_controlupdate > CONTROLUPDATEPERIOD) {
|
||||
last_controlupdate = loopmillis;
|
||||
static unsigned long last_controlupdate_disarmed=0;
|
||||
if (loopmillis - last_controlupdate_disarmed > CONTROLUPDATEPERIOD) {
|
||||
last_controlupdate_disarmed = loopmillis;
|
||||
set_speed = set_speed*(1.0-filter_stop_set_speed); //simple Filter
|
||||
set_steer = set_steer*(1.0-filter_stop_set_steer);
|
||||
int16_t _out_speedl,_out_speedr;
|
||||
|
|
@ -624,11 +647,10 @@ void loop() {
|
|||
|
||||
|
||||
|
||||
if (esc.sendPending(loopmillis)) {
|
||||
|
||||
if (!motorenabled) {//motors disabled. Used for safety off when remote released
|
||||
if (loopmillis - last_controlupdate > CONTROLUPDATEPERIOD) {
|
||||
last_controlupdate = loopmillis;
|
||||
static unsigned long last_controlupdate_motordisabled=0;
|
||||
if (loopmillis - last_controlupdate_motordisabled > CONTROLUPDATEPERIOD) {
|
||||
last_controlupdate_motordisabled = loopmillis;
|
||||
set_speed = set_speed*(1.0-filter_stop_set_speed); //simple Filter
|
||||
set_steer = set_steer*(1.0-filter_stop_set_steer);
|
||||
int16_t _out_speedl,_out_speedr;
|
||||
|
|
@ -637,24 +659,6 @@ void loop() {
|
|||
esc.setSpeed(_out_speedl,_out_speedr);
|
||||
}
|
||||
}
|
||||
last_send = loopmillis;
|
||||
|
||||
|
||||
#ifdef DEBUG
|
||||
Serial.print(" out_speedl=");
|
||||
Serial.print(out_speedl);
|
||||
Serial.print(" out_speedr=");
|
||||
Serial.print(out_speedr);
|
||||
Serial.print(" checksum=");
|
||||
Serial.print(out_checksum);
|
||||
|
||||
Serial.print(" controlmode=");
|
||||
Serial.print(controlmode);
|
||||
|
||||
Serial.println();
|
||||
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
|
@ -674,7 +678,8 @@ void loop() {
|
|||
dataFile.print("current_L,current_R,");
|
||||
dataFile.print("rpm_L,rpm_R,");
|
||||
dataFile.print("temp,vbat,");
|
||||
dataFile.println("trip,currentConsumed");
|
||||
dataFile.println("trip,currentConsumed,motorenabled,disarmedByDelay");
|
||||
dataFile.print("#TIMESTAMP:"); dataFile.println(now());
|
||||
logging_headerWritten=true;
|
||||
}
|
||||
dataFile.print(String(loopmillis)); dataFile.print(";");
|
||||
|
|
@ -687,7 +692,9 @@ void loop() {
|
|||
dataFile.print(esc.getFeedback_boardTemp()); dataFile.print(";");
|
||||
dataFile.print(esc.getFeedback_batVoltage()); dataFile.print(";");
|
||||
dataFile.print(esc.getTrip()); dataFile.print(";");
|
||||
dataFile.print(esc.getCurrentConsumed(),3);
|
||||
dataFile.print(esc.getCurrentConsumed(),3); dataFile.print(";");
|
||||
dataFile.print(motorenabled); dataFile.print(";");
|
||||
dataFile.print(debug_count_disarmedbecauseofnrfdelay);
|
||||
dataFile.println("");
|
||||
dataFile.close();
|
||||
}
|
||||
|
|
@ -1009,6 +1016,10 @@ void display_show_stats3() {
|
|||
display.print(F("nrfdata x,y=")); display.print(lastnrfdata.steer); display.print(F(", ")); display.println(lastnrfdata.speed);
|
||||
display.print(F("nrfdata command=")); display.println(lastnrfdata.commands);
|
||||
|
||||
display.print(F("disarmed delay#=")); display.println(debug_count_disarmedbecauseofnrfdelay);
|
||||
|
||||
|
||||
|
||||
|
||||
display.display(); // Show initial text
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue