improve disable logging check and add experimental tanksteering

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!");
+  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_FL=_cmd_send_toMotor;
-    cmd_send_toMotor-=brake_pos*reverse_speed;
+  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);
+  escFront.setSpeed(cmd_send_toMotor_FL,cmd_send_toMotor_FR);
-  escRear.setSpeed(cmd_send_toMotor,cmd_send_toMotor);
+  escRear.setSpeed(cmd_send_toMotor_RL,cmd_send_toMotor_RR);
 
   log_update=true;
 }