Upload New File

initial upload

sufst-shifter/Shifter_Soton_Rev10.ino

+//Note: I think shift directions are backwards (right button should be from 2-3, 3-4, etc.)
+// "Upshift" in the code moves the actuator backwards then forwards - this goes from 2-3 - This corresponds to the right button input.
+#include <Servo.h>
+#include <millisDelay.h>
+#include <PID_v1.h>
+//#include <Serial.h>
+
+// Define arduino pins
+#define button1 8 // downshift
+#define button2 6 // upshift
+#define button3 12 // neutrasl
+#define shiftCutPin 10 // Shift Cut pin
+#define motorOut 3
+#define pot A0
+#define neutral_pin 5 // Engine Neutral Sensor
+
+//#define DEBUG // Uncomment to enable serial output
+//#define PRINT_POT_POSITION // Uncomment to continuously print the potentiometer position (only works in DEBUG)
+
+// BUTTON STUFF
+int debounce = 10; // 10ms debounce time
+
+
+// POSITION TARGETS
+/*
+int upPos = 650; // upshift distance - max = 250
+int downPos = 350; //downshift distance - max = 770 // changed from 360 down to 300
+int neutral1 = 625; //neutral from first gear distance
+int neutral2 = 425; // neutral from second gear distance
+int middlePos = 514; // middle position on the shifter*/
+
+int upPos = 870; // upshift distance - max = 250
+int downPos = 510; //downshift distance - max = 770 // changed from 360 down to 300
+int neutral1 = 810; //neutral from first gear distance
+int neutral2 = 570; // neutral from second gear distance
+int middlePos = 700; // middle position on the shifter //was 715
+int first = 490; //First gear from Second gear
+int second = 870; //Second gear from First gear
+
+// middle = 765;
+// neutral1 = 855;
+// neutral2 = 610;
+// upPos = 865;
+// downPos = 600;
+
+// GENERAL SETTINGS
+Servo motor;
+int neutral = 1500;
+int bumpMoveSpeed = 200; // Max speed is +-500
+int normalMoveSpeed = 450; // Max speed is +-500
+int normalReturnSpeed = 350; // Max speed is +-500 // changed from 200 to 300
+int pidMoveSpeed = 500; // Max speed is +-500
+int minSpeed = 0;
+
+int cutoff = 100; // stop running after 100ms of stall
+int stallDist = 20;
+int modeSwitchTime = 1000;
+int nLoop = 0;
+
+// BUMP MODE
+#define BUMP_NEUTRAL // Comment out to disable neutral in bump mode
+float bumpReturnBodge = 0.83;
+int bumpModeTime = 250;
+
+
+// NORMAL MODE
+int startBuffer = 10;
+int normalSlowdown = 150; // distance from shift pos to start slowing down
+int returnSlowdown = 150; // distance from shift pos to start slowing down
+int move_speed = 93; // initialize servo output as no movement
+int targetPos = middlePos;
+int delta = 0;
+int currentSpeed = neutral;
+int neutral_flag;
+int stop_buffer = 0;
+int stall_limit = 75;
+
+// PID MODE
+double Setpoint = middlePos;
+double Input, Output;
+double intermediateControl;
+double Kp = 2.5, Ki = 0, Kd = 0.05;
+PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
+int deadband = 5;
+bool ret = false;
+
+
+
+// INITIALISE MORE VARIABLES
+int shiftTask = 0; // define shift as no shift for initialization
+int shiftMode = 2; // 0 - Normal, 1 - Bump, 2 - PID
+int button = 0;
+int slowdownFactor = 1;
+int moveDir = 0;
+bool delayRunning = false; // true if still waiting for delay to finish
+unsigned long delayStart = 0;
+int startingPos = analogRead(pot);
+int currentPos = startingPos;
+int gear = 1; // assume gear starts in neutral
+
+
+// Set counters
+int count = 0;
+
+
+
+
+void setup() {
+  // Set up button pins
+  pinMode(button1, INPUT_PULLUP);
+  pinMode(button2, INPUT_PULLUP);
+  pinMode(button3, INPUT_PULLUP);
+  pinMode(neutral_pin, INPUT_PULLUP);
+  pinMode(shiftCutPin, OUTPUT);
+
+  digitalWrite(shiftCutPin, HIGH);
+  // Attach motor controller
+  motor.attach(motorOut);
+  motor.writeMicroseconds(neutral);
+  
+  myPID.SetMode(AUTOMATIC);
+  motor.writeMicroseconds(1500);
+  myPID.SetOutputLimits(-pidMoveSpeed, pidMoveSpeed);
+  myPID.SetSampleTime(3);
+
+  delayStart = millis();
+  delayRunning = true;
+
+
+#ifdef DEBUG
+  // Initiate Serial Output
+  Serial.begin(115200);
+  Serial.println("Serial Ready!");
+  printShiftMode();
+#endif
+}
+
+#ifdef DEBUG
+  void printShiftMode(){
+    if (shiftMode == 0) {
+      Serial.println("Normal Shift Mode Activated");
+    }
+    else if (shiftMode == 1) {
+      Serial.println("Bump Shift Mode Activated");
+    }
+    else if (shiftMode == 2) {
+      Serial.println("PID Shift Mode Activated");
+    }
+  }
+#endif
+
+
+void checkRequestedMode() {
+  // Check which button is pressed and set the shift mode
+  if (!digitalRead(button1)) {
+    button = button1;
+    shiftTask = 1;
+    Kp = 3.5, Ki = 0, Kd = 0.05;
+  }
+  else if (!digitalRead(button2)) {
+    button = button2;
+    shiftTask = 2;
+    Kp = 2.5, Ki = 0, Kd = 0.05;
+  }
+  else if (!digitalRead(button3)) {
+    button = button3;
+    shiftTask = 3;
+  }
+  else {
+    button = 999;
+    shiftTask = 0;
+  }
+
+  // If any button is pressed check if it was pressed long enough
+  if (button != 999) {
+    count = 0;
+
+    while (!digitalRead(button)) {
+      delay(1);
+      count++;
+    }
+
+    if (count < debounce) {
+      shiftTask = 0;
+    }
+    else if (shiftTask == 3 && count >= modeSwitchTime) {
+      shiftTask = 0;
+      if (shiftMode >= 2) {
+        shiftMode = 0;
+      }
+      else {
+        shiftMode++;
+      }
+      
+    #ifdef DEBUG
+      printShiftMode();
+    #endif
+    }
+  }
+}
+
+
+
+
+
+
+void bumpShift() {
+  if (shiftTask == 1) { // Downshift
+    moveDir = 1;
+    slowdownFactor = 1;
+  }
+  else if (shiftTask == 2) { // Upshift
+    moveDir = -1;
+    slowdownFactor = 1;
+  }
+#ifdef BUMP_NEUTRAL
+  else if (shiftTask == 3) { // Go to neutral
+    if (gear == 2) { // From 1st
+      moveDir = -1;
+      slowdownFactor = 2;
+    }
+    else if (gear == 1) { // From 2nd
+      moveDir = 1;
+      slowdownFactor = 2;
+    }
+    else { // Do nothing
+      return;
+    }
+  }
+#endif
+  else { // Do nothing
+    return;
+  }
+
+
+  #ifdef DEBUG
+    printBumpDebug(0, 0);
+  #endif
+  
+  // Move towards a gearshift
+  motor.writeMicroseconds(neutral + moveDir * bumpMoveSpeed / slowdownFactor);
+  
+  #ifdef DEBUG
+    printBumpDebug(neutral + moveDir * bumpMoveSpeed / slowdownFactor, bumpModeTime);
+  #endif
+  
+  delay(bumpModeTime);
+  
+  // Wait a bit
+  motor.writeMicroseconds(neutral);
+  
+  #ifdef DEBUG
+    printBumpDebug(neutral, 50);
+  #endif
+  
+  delay(50);
+  
+  // Move back
+  moveDir = -1 * moveDir;
+  motor.write(neutral + bumpReturnBodge * moveDir * bumpMoveSpeed / slowdownFactor);
+  
+  #ifdef DEBUG
+    printBumpDebug(neutral + bumpReturnBodge * moveDir * bumpMoveSpeed / slowdownFactor, bumpModeTime);
+  #endif
+  
+  delay(bumpModeTime);
+}
+
+
+
+void pidShift() {
+  if (!digitalRead(neutral_pin)){
+    gear = 0;
+  }
+  if (shiftTask == 1) { // downshift
+    if (gear == 2){
+      Setpoint = first;
+    }
+    else {
+      Setpoint = downPos;
+    }
+  }
+  else if (shiftTask == 2) { // upshift
+    Setpoint = upPos;
+  }
+  else if (shiftTask == 3) { // Go to neutral
+    if (gear == 1) { // From 1st
+      Setpoint = neutral1;
+    }
+    else if (gear == 2) { // from 2nd
+      Setpoint = neutral2;
+    }
+  else { // Do nothing
+    return;
+  }
+  }
+  else { // Do nothing
+    return;
+  }
+
+  myPID.SetTunings(Kp,Ki,Kd);
+
+  pidLoop();
+
+  Setpoint = middlePos;
+  
+  Kp = 2.0, Ki = 0, Kd = 0.075;
+
+  myPID.SetTunings(Kp,Ki,Kd);
+
+  pidLoop();
+}
+
+
+
+
+void pidLoop() {
+  Input = analogRead(pot);
+  startingPos = Input;
+  count = 0;
+  nLoop = 0;
+  while (abs(Input - Setpoint) > deadband) {
+    nLoop++;
+      if (Setpoint == downPos && !digitalRead(neutral_pin)) {
+        Setpoint = first;
+        Serial.println("Going to First");
+      }
+      else if (targetPos == upPos && !digitalRead(neutral_pin)) {
+        Setpoint = second;
+        Serial.println("Going to Second");
+      }
+    Input = analogRead(pot);
+    ret = false;
+    while (ret == false){
+    ret = myPID.Compute();
+    }
+    intermediateControl = Output - 128;
+    intermediateControl /= 128;
+    #ifdef DEBUG
+      //Serial.print("Control Output: ");
+      //Serial.println(1500 - Output);
+      //Serial.print("Ana input: ");
+      //Serial.println(Input);
+    #endif
+    motor.writeMicroseconds(1500 - Output);
+    if (abs(startingPos - Input) < 3 && nLoop > 5){
+        if (count > stall_limit) {
+          antiStall();
+          break;
+        }
+        else {
+          count++;
+          #ifdef DEBUG
+          
+            Serial.println(abs(startingPos - Input));
+            Serial.println(count);
+          #endif
+        }
+      }
+      else{
+        startingPos = Input;
+        count = 0;
+      }
+  }
+
+}
+
+
+
+
+
+void loop() {
+  //gear = 2; // assume gear is always 2nd (until we get gear number from CAN)
+  #ifdef DEBUG
+      //Serial.print("Gear no: ");
+      //Serial.println(gear);
+    #ifdef PRINT_POT_POSITION
+      Serial.println(analogRead(pot));
+    #endif
+  #endif
+  // Read Button inputs - should be replaced by CAN read
+  checkRequestedMode();
+
+  if (shiftTask != 0) {
+    if (shiftMode == 0) { // Normal Mode
+      normalShift();
+    }
+    else if (shiftMode == 1) { // Bump Mode
+      bumpShift();
+    }
+    else if (shiftMode == 2) { // PID Mode
+      pidShift();
+    }
+    
+    // Stop the motor
+    motor.writeMicroseconds(neutral);
+    #ifdef DEBUG
+      printStopDebug();
+      delay(500);
+      printStopDebug();
+      Serial.println();
+    #endif
+  }
+
+
+}
+
+
+void normalShift() {
+  if (!digitalRead(neutral_pin)){
+    gear = 0;
+  }
+  else{
+    gear = 1; // Assume gear is 1 until CAN works
+  }
+  if (shiftTask == 1) { // Downshift
+    moveDir = 1;
+    targetPos = downPos;
+    #ifdef DEBUG
+      Serial.print("Gear no: ");
+      Serial.println(gear);
+    #endif
+  }
+  else if (shiftTask == 2) { // Upshift
+    moveDir = -1;
+    targetPos = upPos;
+    #ifdef DEBUG
+      Serial.print("Gear no: ");
+      Serial.println(gear);
+    #endif
+  }
+  else if (shiftTask == 3) { // Go to neutral
+    if (gear == 1) { // From 1st
+      moveDir = -1;
+      targetPos = neutral1;
+      #ifdef DEBUG
+        Serial.println("Going to neutral from 1st");
+      #endif
+    }
+    else if (gear == 2) { // From 2nd
+      moveDir = 1;
+      targetPos = neutral2;
+      #ifdef DEBUG
+        Serial.println("Going to neutral from 2nd");
+      #endif
+    }
+    else { // Do nothing
+      return;
+    }
+  }
+  else { // Do nothing
+    return;
+  }
+
+  currentPos = analogRead(pot); // get current position
+  startingPos = currentPos;
+  delayStart = millis(); // initiliaze stall timer
+  
+  #ifdef DEBUG
+    Serial.print("Target: ");
+    Serial.print(targetPos);
+    Serial.print(" | Move Direction: ");
+    Serial.print(moveDir);
+    Serial.print(" | ");
+    Serial.print((currentPos - targetPos) * moveDir);
+    Serial.println();
+    printNormalDebug();
+  #endif
+
+  if (shiftTask == 2) {
+    digitalWrite(shiftCutPin, LOW);
+  }
+  normalShiftLoop(normalMoveSpeed, normalSlowdown, gear);
+  
+  //motor.writeMicroseconds(neutral);
+  
+  moveDir = -1 * moveDir;
+  targetPos = middlePos+75*moveDir;
+
+  #ifdef DEBUG
+    Serial.print("Target: ");
+    Serial.print(targetPos);
+    Serial.print(" | Move Direction: ");
+    Serial.print(moveDir);
+    Serial.print(" | ");
+    Serial.print((currentPos - targetPos) * moveDir);
+    Serial.println();
+    printNormalDebug();
+  #endif
+  
+  digitalWrite(shiftCutPin, HIGH);
+  
+  //Return to middle
+  normalShiftLoop(normalReturnSpeed, returnSlowdown, gear);
+
+  //Return to middle after return because it always overruns
+  moveDir = -1 * moveDir;
+  targetPos = middlePos;
+
+  normalShiftLoop(200,50, gear);
+  
+}
+void normalShiftLoop (int lMoveSpeed, int slowdown, int gear){
+  startingPos = analogRead(pot);
+  delta = (startingPos - targetPos) * moveDir;
+  #ifdef DEBUG
+    if (delta < 0) {
+      Serial.println("Delta is wrong");
+    }
+  #endif
+  if (delta >= stop_buffer){
+    // Do max speed initially to start moving
+    motor.writeMicroseconds(neutral + moveDir*500);
+    delay(3);
+    nLoop = 0;
+    count = 0;
+    while (delta > 0) {
+      if (targetPos == downPos && !digitalRead(neutral_pin) && gear != 0) {
+        targetPos = targetPos - 25;
+        Serial.println("Going to First");
+      }
+      else if (targetPos == upPos && !digitalRead(neutral_pin)&& gear != 0) {
+        targetPos = targetPos + 25;
+        Serial.println("Going to Second");
+      }
+      currentPos = analogRead(pot);
+      if (delta > slowdown || (startingPos - currentPos) * moveDir <= startBuffer) { // full speed
+        currentSpeed = neutral + moveDir * lMoveSpeed;
+      }
+      else { // slow it down as it gets closer to target
+        currentSpeed = neutral + moveDir * round(lMoveSpeed * (float) delta / slowdown) ;
+      }
+  
+      #ifdef DEBUG
+        printNormalDebug();
+      #endif
+      if (abs(startingPos - currentPos) < 3 && nLoop > 5){
+        if (count > 7) {
+          antiStall();
+          break;
+        }
+        else {
+          currentSpeed = neutral + moveDir * lMoveSpeed;
+          count++;
+          #ifdef DEBUG
+          
+            Serial.println(abs(startingPos - currentPos));
+            Serial.println(count);
+          #endif
+        }
+      }
+      motor.writeMicroseconds(currentSpeed);
+      
+      delta = (currentPos - targetPos) * moveDir;
+      nLoop++;
+      startingPos = currentPos;
+  /*
+      if (abs(currentPos - startingPos) < stallDist){
+        if (millis() - delayStart > cutoff) {
+          #ifdef DEBUG
+            Serial.println("Stalled");
+          #endif
+          motor.write(neutral);
+          delay(500);
+          break;
+        }
+      }
+      else {
+        startingPos = currentPos;
+        delayStart = millis();
+      }*/
+    }
+  }
+}
+void antiStall(){
+  #ifdef DEBUG
+    Serial.println("Stalled");
+  #endif
+  motor.writeMicroseconds(neutral);
+}
+
+
+
+#ifdef DEBUG
+void printState(char message, int gear, int pos, int shiftTask) {
+  if (strlen(message) > 0) {
+    Serial.println(message);
+  }
+
+  // Print current gear, current shift task, and current pot position
+  Serial.print("Current Gear is: ");
+  Serial.print(gear);
+  Serial.println("");
+  Serial.print("Current Pot Position is: ");
+  Serial.print(pos);
+  Serial.println("");
+  Serial.print("Current Shift Task is:");
+  Serial.print(shiftTask);
+  Serial.println("");
+  Serial.println("Current Direction is Up");
+  Serial.println("");
+}
+
+
+void printBumpDebug(int bumpSpeed, int bumpDelay){
+  Serial.print("Bump Mode - ");
+  if (shiftTask == 1) { // upshift
+    Serial.print("Downshift");
+  }
+  else if (shiftTask == 2) { // downshift
+    Serial.print("Upshift");
+  }
+  else if (shiftTask == 3) { // go to neutral
+    if (gear == 1) { // from 1st
+      Serial.print("Neutral from 1st");
+    }
+    else if (gear == 2) { // from 2nd
+      Serial.print("Neutral from 2nd");
+    }
+  }
+  Serial.print(" | Pulse Width: ");
+  Serial.print(bumpSpeed);
+  Serial.print("ms | Delay: ");
+  Serial.print(bumpDelay);
+  Serial.print(" | Position: ");
+  Serial.print(analogRead(pot));
+  
+  Serial.println();
+}
+
+
+void printNormalDebug(){
+  Serial.print("Normal Mode - ");
+  if (shiftTask == 1) { // upshift
+    Serial.print("Downshift");
+  }
+  else if (shiftTask == 2) { // downshift
+    Serial.print("Upshift");
+  }
+  else if (shiftTask == 3) { // go to neutral
+    if (gear == 1) { // from 1st
+      Serial.print("Neutral from 1st");
+    }
+    else if (gear == 2) { // from 2nd
+      Serial.print("Neutral from 2nd");
+    }
+  }
+  Serial.print(" | Pulse Width: ");
+  Serial.print(currentSpeed);
+  Serial.print("ms | Position Target: ");
+  Serial.print(targetPos);
+  Serial.print(" | Position: ");
+  Serial.print(currentPos);
+  Serial.print(" | Delta: ");
+  Serial.print(delta);
+  
+  Serial.println();
+}
+
+
+void printStopDebug(){
+  Serial.print("Debug Mode - ");
+  Serial.print("Stop");
+  Serial.print(" | Pulse Width: ");
+  Serial.print(neutral);
+  Serial.print("ms | Position: ");
+  Serial.print(analogRead(pot));
+  
+  Serial.println();
+}
+#endif