From 156455f363dbd340ccd79df7d5fe098598a86087 Mon Sep 17 00:00:00 2001 From: Fisch Date: Sun, 14 Jul 2024 12:30:20 +0200 Subject: [PATCH] improve disable logging check and add experimental tanksteering --- controller_teensy/src/main.cpp | 124 +++++++++++++++++++++++++++++---- 1 file changed, 112 insertions(+), 12 deletions(-) diff --git a/controller_teensy/src/main.cpp b/controller_teensy/src/main.cpp index a0384d5..275c94b 100644 --- a/controller_teensy/src/main.cpp +++ b/controller_teensy/src/main.cpp @@ -72,9 +72,22 @@ void setup() pinMode(PIN_PWRBUTTON, INPUT_PULLUP); //Pressed=High - if (!digitalRead(PIN_PWRBUTTON)) { //button is not pressed during startup means teensy is powered externally (usb) - datalogging=false; //disable logging when connected via usb to not clutter up sd card - Serial.println("PWRBUTTON not pressed. Logging disabled!"); + + datalogging=false; //disable logging when connected via usb to not clutter up sd card + Serial.print("PWRBUTTON "); + for (uint8_t i=0;i<10;i++) { //check a few times + if (!digitalRead(PIN_PWRBUTTON)) { //button is not pressed during startup means teensy is powered externally (usb) + Serial.print("."); + delay(100); //give more time to disable logging + }else{ + datalogging=true; //button presse at least once recognized. enable logging + Serial.print("X"); + } + } + if (!datalogging){ + Serial.println(" not pressed. Logging disabled!"); + }else{ + Serial.println(" Pressed. Logging enabled"); } pinMode(PIN_LED_START, OUTPUT); //Active High @@ -92,7 +105,7 @@ void setup() led_testLEDSBlocking(); - delay(2000); + delay(100); Serial.println("Init Functions"); led_simpeProgress(0,1); @@ -519,27 +532,114 @@ void calculateSetSpeed(unsigned long timediff){ } - //acceleration + // ## Acceleration cmd_send += constrain(adjusted_throttle_pos-cmd_send,0,(int16_t)(max_acceleration_rate*(timediff/1000.0)) ); //if throttle higher than last applied value, apply throttle directly with rate limit cmd_send=constrain(cmd_send,0,throttle_max); last_cmd_send=cmd_send; - int16_t cmd_send_toMotor=constrain(cmd_send* (1.0-(brake_pos*0.5/1000.0) ) ,0,throttle_max); //brake "ducking" + int16_t cmd_send_toMotor_FL=0; + int16_t cmd_send_toMotor_FR=0; + int16_t cmd_send_toMotor_RL=0; + int16_t cmd_send_toMotor_RR=0; + int16_t _cmd_send_toMotor=constrain(cmd_send* (1.0-(brake_pos*0.5/1000.0) ) ,0,throttle_max); //brake "ducking" - - if (reverse_enabled) { - cmd_send_toMotor-=brake_pos*reverse_speed; + + cmd_send_toMotor_FL=_cmd_send_toMotor; + cmd_send_toMotor_FR=_cmd_send_toMotor; + cmd_send_toMotor_RL=_cmd_send_toMotor; + cmd_send_toMotor_RR=_cmd_send_toMotor; + + + + + // ## Experimental Tank steering + static float tanksteering_differential=0; //to ramp up slowly. value between -1.0 and 1.0 + //Parameters: + int16_t tanksteering_max_speed=200; + float tanksteering_rate_increase=1.0; //increase units per second + float tanksteering_rate_decrease=1.5; //decrease units per second + + + + if (control_buttonA && !control_buttonB && !reverse_enabled && throttle_pos<=0) { //Right button (A) only. and throttle touched + tanksteering_differential+=tanksteering_rate_increase*(timediff/1000.0); + tanksteering_differential=constrain(tanksteering_differential,-1.0,1.0); //constrain between 0 and 1 + }else if(control_buttonB && !control_buttonA && !reverse_enabled && throttle_pos<=0) { //Left button (B) only. and throttle touched + tanksteering_differential-=tanksteering_rate_increase*(timediff/1000.0); + tanksteering_differential=constrain(tanksteering_differential,-1.0,1.0); //constrain between 0 and 1 + }else{ //buttons released + if(tanksteering_differential>0) { + tanksteering_differential-=tanksteering_rate_decrease*(timediff/1000.0); + }else if(tanksteering_differential<0){ + tanksteering_differential+=tanksteering_rate_decrease*(timediff/1000.0); + } + tanksteering_differential=constrain(tanksteering_differential,-1.0,1.0); //constrain between 0 and 1 } + cmd_send_toMotor_FL+=tanksteering_differential*tanksteering_max_speed; + cmd_send_toMotor_FR-=tanksteering_differential*tanksteering_max_speed; + cmd_send_toMotor_RL+=tanksteering_differential*tanksteering_max_speed; + cmd_send_toMotor_RR-=tanksteering_differential*tanksteering_max_speed; + + + + // ## Braking, Reversing and Standstill movements below here ## + if (reverse_enabled) { //backwards driving not prohibited + _cmd_send_toMotor-=brake_pos*reverse_speed; + + cmd_send_toMotor_FL=_cmd_send_toMotor; + cmd_send_toMotor_FR=_cmd_send_toMotor; + cmd_send_toMotor_RL=_cmd_send_toMotor; + cmd_send_toMotor_RR=_cmd_send_toMotor; + + } + + /* + float steeringdifferential_speed=0.1; //Speed for turning the steering wheel by differtially driving the front wheels + + static bool reverse_enabled2=true; //pohibit instant reverse driving when releasing steering wheel buttons when brake is still pressed. wait for throttle and brake to release once + if (!reverse_enabled2 && throttle_pos<=0 && brake_pos<=0){ + reverse_enabled2=true; + } + + if (reverse_enabled) { //backwards driving not prohibited + if (control_buttonA && !control_buttonB) { //Right button (A) only + cmd_send_toMotor_FL+=brake_pos*steeringdifferential_speed; + cmd_send_toMotor_FR-=brake_pos*steeringdifferential_speed; + //cmd_send_toMotor_RL-=0; + //cmd_send_toMotor_RR-=0; + reverse_enabled2=false; + + }else if(control_buttonB && !control_buttonA) { //Left button (B) only + cmd_send_toMotor_FL-=brake_pos*steeringdifferential_speed; + cmd_send_toMotor_FR+=brake_pos*steeringdifferential_speed; + //cmd_send_toMotor_RL-=0; + //cmd_send_toMotor_RR-=0; + reverse_enabled2=false; + + }else if(reverse_enabled2){ //no button on steering wheel pressed, drive backwards + _cmd_send_toMotor-=brake_pos*reverse_speed; + + cmd_send_toMotor_FL=_cmd_send_toMotor; + cmd_send_toMotor_FR=_cmd_send_toMotor; + cmd_send_toMotor_RL=_cmd_send_toMotor; + cmd_send_toMotor_RR=_cmd_send_toMotor; + } + } + */ + if (!controllers_connected || !armed) { //controllers not connected or not armed cmd_send=0; - cmd_send_toMotor=0; //safety off + cmd_send_toMotor_FL=0; //safety off + cmd_send_toMotor_FR=0; //safety off + cmd_send_toMotor_RL=0; //safety off + cmd_send_toMotor_RR=0; //safety off } - escFront.setSpeed(cmd_send_toMotor,cmd_send_toMotor); - escRear.setSpeed(cmd_send_toMotor,cmd_send_toMotor); + escFront.setSpeed(cmd_send_toMotor_FL,cmd_send_toMotor_FR); + escRear.setSpeed(cmd_send_toMotor_RL,cmd_send_toMotor_RR); log_update=true; }