Diff: challenge.cpp

Revision:

0:4788e1df7b55

Child:

1:ba85dae98035

diff -r 000000000000 -r 4788e1df7b55 challenge.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/challenge.cpp	Fri Mar 13 11:48:04 2020 +0000
@@ -0,0 +1,284 @@
+#include <mbed.h>
+#include "definitions.h"
+#include "challenge.h"
+#include "remoteControl.h"
+#include "dashboard.h"
+#include "motor.h"
+
+ChallengeMode::ChallengeMode(InterruptIn& autoStopTrigger, Dashboard& dashboard, Remote& remote, Motor& motor1) : 
+                            _autoStopTrigger(autoStopTrigger), _dashboard(dashboard), _remote(remote), _motor1(motor1) {
+    // CONSTRUCTOR
+                                
+    // ATTACH AUTOSTOP INTERRUPT
+    _autoStopTrigger.rise(this, &ChallengeMode::autoStopTriggered);
+    
+    // FUNCTIONS
+    regenThrottleOff(); // Make sure regen throttle is off
+    autoStopOff();      // Make sure auto-stop is off
+    innovationOff();    // make sure innovation (collision detection mode) is off
+    
+    // VARIABLES
+    
+    contactBatt = 1;
+    
+    // Auto-Stop Challenge
+    autoStopActive = false;             // Flag is auto-stop mode is active
+    autoStopInProgress = false;         // Flag if auto-stop track-side sensor has been detected and auto-stop is in progress
+    autoStopCruiseSpeed = 0;            // Speed of loco at time of auto-stop trackside sensor triggering so loco can maintain this speed without operator input
+    autoStopThrottle = 0;               // Throttle rate applied at the time of triggering autostop
+    targetDistance = 25.00f;            // How far in meters to bring the loco to a stop
+    remainingDistance = targetDistance; // How far the loco has left to go before reaching target distance
+    decelerationGradient = 0;           // Gradient of y=mx+c linear speed-distance curve that is used in this auto-stop version
+    requiredSpeed = 0;                  // How fast the loco should be going according to y=mx+c line
+    
+    // Innovation Challenge
+    innovationDistanceLimit = 1500;     // Stopping distance in mm from IR sensors
+    stopLoco = false;                   // Flag to stop the loco when obstacle has been detected
+    
+    // IR Output voltages at various distances (for calibration purposes)
+    voltageAt5500 = 0;
+    voltageAt5000 = 0;
+    voltageAt4500 = 0;
+    voltageAt4000 = 0;
+    voltageAt3500 = 0;
+    voltageAt3000 = 0;
+    voltageAt2500 = 0;
+    voltageAt2000 = 0;
+    voltageAt1500 = 0;
+    voltageAt1000 = 2.2f;
+    voltageAt500  = 3.0f;
+    
+}    // CONSTRUCTOR
+
+void ChallengeMode::regenThrottleOn() {
+    // OPEN THE BATTERY CONTACTOR SO POWER IS DELIVERED BY SUPERCAPS
+    
+    contactCapCharge = 0;               // Open supercap charging contactor to prevent charging
+    
+    contactBatt = 0;                    // Open battery contactor so all power comes from supercaps
+    regenThrottleActive = true;         // Flag to indicate that regen throttling is on
+    pc.printf("Regen Throttle On\r\n");
+}
+
+void ChallengeMode::regenThrottleOff() {
+    // CLOSE THE BATTERY CONTACTOR
+//    if (regenBrakingActive == false) {  // If regen braking is not active and using the supercaps, allow capacitor to pre-charge from batteries
+//        contactCapCharge = 1;           // Close the supercap charging contactor
+//    }
+    
+    contactBatt = 1;                    // Close the battery contactor so power comes from batteries
+    regenThrottleActive = false;        // Flag to indicate that regen throttling is off
+    pc.printf("Regen Throttle Off\r\n");
+}
+
+bool ChallengeMode::regenBrakingOn() {
+    // TURN ON REGEN BRAKING
+    if (superCapVoltage == 0) {             // If super caps are not full
+        contactCapCharge = 0;               // Open the super cap pre-cahrging contactor
+        contactBatt = 0;                    // Open battery contactor so all power comes from supercaps
+        regenBrakingActive = true;          // Flag to indicate that regen throttling is on
+        pc.printf("Regen Braking On\r\n");
+        return 1;                           // Return 1 if regen braking switched on
+    }
+    else {
+        return 0;                           // Return 0 is regen braking didnt turn on due to full supercaps
+    }
+}
+
+void ChallengeMode::regenBrakingOff() {
+    // TURN OFF REGEN BRAKING
+    if (regenThrottleActive == false) {     // If regen throttle is not active and using the supercaps, allow capacitor to pre-charge from batteries
+        contactCapCharge = 1;               // Close the supercap pre-charge contactor
+    }
+    regenBrakingActive = false;             // Flag that regen braking is off
+    contactBatt = 1;                    // Close battery contactor so all power comes from supercaps
+    pc.printf("Regen Braking Off\r\n");
+}
+
+void ChallengeMode::autoStopOn() {
+    // TURN ON AUTOSTOP MODE
+    autoStopActive = true;                  // Flag that auto-stop mode is on
+    pc.printf("Auto-Stop On\r\n");
+}
+
+void ChallengeMode::autoStopTriggered() {
+    // INTERRUPT FUNCTION CALLED WHEN TRACK-SIDE TRIGGER HAS BEEN DETECTED
+    if (autoStopActive == true && autoStopInProgress == false) {    // If auto-stop mode is on and signal has not yet been detected
+        autoStopInProgress = true;                                  // Flag that auto-stop is in progress and fully autonomous
+        autoStopCruiseSpeed = _dashboard.currentSpeed;              // Set the speed the loco was going at the point of triggering
+        autoStopThrottle = _remote.throttle;
+        
+        _dashboard.currentDistance = 0.00f;                         // Reset the distance-travelled counter to 0
+//        timeToReachTarget = targetDistance / (autoStopCruiseSpeed * 1000 / 3600);   // Time(s) = Distance(m) / Speed(converted to m/s)
+        decelerationGradient = (autoStopCruiseSpeed - 1) / (targetDistance - 1);    // m = y / x → to get to 1kph at 24m, leaving ~ 4 seconds to get to target of 25m
+        pc.printf("Auto-Stop Triggered\r\n");
+    }
+}
+
+void ChallengeMode::autoStopControl() {
+    // FUNCTION TO MANAGE THE LOCO THROTTLE AND BRAKING WHEN AUTO-STOPPING
+    remainingDistance = targetDistance - _dashboard.currentDistance;        // Calculate remaining distance from target distance
+//    timeToReachTarget = int(_dashboard.currentSpeed) > 0 ? remainingDistance / _dashboard.currentSpeed : 999;
+    
+    // FOLLOWING DECELERATION GRADIENT
+    
+    if (remainingDistance > 2.0f) {    // IF OVER 3M FROM TARGET, CONTROL SPEED (1m + 2m sensor to nose of train)
+    
+        _motor1.throttle(0.3f);     // Apply 30% throttle
+        //requiredSpeed = (decelerationGradient * _dashboard.currentDistance) + autoStopCruiseSpeed;  // (y = mx + c)
+//        
+//        if (_dashboard.currentSpeed < requiredSpeed) {  // If train is below the speed it should be, apply 30% throttle and no braking
+////            _motor1.throttle(0.3f);
+////            _motor2.throttle(0.1f);
+////            _motor1.brake(0.0f);
+////            _motor2.brake(0.0f);
+//
+//            brakeValve32 = 1;
+//        }
+//        else {                                                              // If loco is going faster than is should be
+//            int speedDifference = _dashboard.currentSpeed - requiredSpeed;  // Work out how much faster it is going and apply a proportional amount of motor braking
+//            
+//            _motor1.throttle(0.0f);         // Turn off throttle
+////            _motor2.throttle(0.0f);
+//
+//            brakeValve32 = 0;
+//            if (pressureSwitch1.read() == 0) {
+//                brakeValve22 = 0;
+//                pc.printf("Pressure 1 Reached");
+//            }
+//            
+//            switch (speedDifference) {  // MOTOR BRAKING
+//                case 1:
+//                    _motor1.brake(0.1f);
+////                    _motor2.brake(0.1f);
+//                    break;
+//                    
+//                case 2:
+//                    _motor1.brake(0.2f);
+////                    _motor2.brake(0.2f);
+//                    break;
+//                    
+//                case 3:
+//                    _motor1.brake(0.3f);
+////                    _motor2.brake(0.3f);
+//                    break;
+//                    
+//                case 4:
+//                    _motor1.brake(0.4f);
+////                    _motor2.brake(0.4f);
+//                    break;
+//                    
+//                case 5:
+//                    _motor1.brake(0.5f);
+////                    _motor2.brake(0.5f);
+//                    break;
+//                    
+//                case 6:
+//                    _motor1.brake(0.6f);
+////                    _motor2.brake(0.6f);
+//                    break;
+//                    
+//                case 7:
+//                    _motor1.brake(0.7f);
+////                    _motor2.brake(0.7f);
+//                    break;
+//                    
+//                case 8:
+//                    _motor1.brake(0.8f);
+////                    _motor2.brake(0.8f);
+//                    break;
+//                    
+//                case 9:
+//                    _motor1.brake(0.9f);
+////                    _motor2.brake(0.9f);
+//                    break;
+//                    
+//                default:
+//                    _motor1.brake(1.0f);
+////                    _motor2.brake(1.0f);
+//                    break;
+//            }   // switch
+//        }// else
+    }// if 
+    else {  // IF REACHED STOPPING TARGET AREA
+        rtc_Trigger = 0;        // APPLY EMERGENCY BRAKES
+//        if (remainingDistance > 0.2f) {     // If more than 20cm to go, stay at 1kph
+//        
+//        }
+//        else {                              // If less than 20cm from target apply full mechanical brakes and turn off throttle
+//            _motor1.throttle(0.0f);
+////            _motor2.throttle(0.0f);
+//            
+//            // MECHANICAL BRAKES
+//            brakeValve32 = 1;
+//            if (pressureSwitch3 == 0) {
+//                brakeValve22 = 0;
+//            }
+//            else {
+//                brakeValve22 = 1;   
+//            }
+//        }
+    }
+    
+    pc.printf("remainingDistance  %f\r\n", remainingDistance);
+//    pc.printf("timeToReachTarget %f\r\n", timeToReachTarget);
+}
+
+void ChallengeMode::autoStopOff() {
+    // TURN OFF AUTOSTOP MODE
+    autoStopActive = false;         // CLEAR AUTOSTOP MODE FLAG
+    autoStopInProgress = false;     // CLEAR AUTOSTOPPING
+    rtc_Trigger = 1;
+    pc.printf("Auto-Stop Off\r\n");
+}
+
+void ChallengeMode::innovationOn() {
+    // TURN ON INNOVATION MODE
+    innovationActive = true;
+    stopLoco = false;
+    rtc_Trigger = 1;
+    _motor1.setSpeedMode(1);     // SET MOTORS TO INCH FORWARD MODE
+//    _motor2.setSpeedMode(1);
+    pc.printf("Innovation On\r\n");
+}
+
+void ChallengeMode::innovationOff() {
+    // TURN OFF INNOVATION MODE
+    innovationActive = false;
+    stopLoco = false;
+    rtc_Trigger = 1;
+    _motor1.setSpeedMode(3);     // SET MOTOR SPEED LIMIT TO MAX
+    pc.printf("Innovation Off\r\n");
+}
+
+int ChallengeMode::innovationControl(int requestedThrottle) {
+    // CONTROL THE TRAIN THROTTLING AND BRAKING
+    
+    int count = 0;
+    
+    if (irSensor_1 > voltageAt1000 / 3.3f) { count++; }
+    if (irSensor_2 > voltageAt1000 / 3.3f) { count++; }
+    if (irSensor_3 > voltageAt1000 / 3.3f ) { count++; }
+    
+    if (count >= 2) {
+        stopLoco = true;
+    }
+
+    
+    if (stopLoco == true) { // IF SENSORS INDICATE TRAIN SHOULD STOP
+    
+//        // APPLY MECHANICAL BRAKES
+//        brakeValve32 = 0;
+//        brakeValve22 = 1;
+
+        // APPLY E-BRAKE
+        rtc_Trigger = 0;
+        
+        pc.printf("Obstacle Detected\r\n");
+        
+        return 0;       // RETURN THROTTLE = 0 TO INDICATE TRAIN SHOULD STOP
+    }
+    else {
+        return requestedThrottle;   // OTHERWISE THROTTLE = USER REQUEST
+    }
+}
\ No newline at end of file