challenge.cpp

00001 #include <mbed.h>
00002 #include "definitions.h"
00003 #include "challenge.h"
00004 #include "remoteControl.h"
00005 #include "dashboard.h"
00006 #include "motor.h"
00007 
00008 ChallengeMode::ChallengeMode(InterruptIn& autoStopTrigger, Dashboard& dashboard, Remote& remote, Motor& motor1) : 
00009                             _autoStopTrigger(autoStopTrigger), _dashboard(dashboard), _remote(remote), _motor1(motor1) {
00010     // CONSTRUCTOR
00011                                 
00012     // ATTACH AUTOSTOP INTERRUPT
00013     _autoStopTrigger.rise(this, &ChallengeMode::autoStopTriggered);
00014     
00015     // FUNCTIONS
00016     regenThrottleOff(); // Make sure regen throttle is off
00017     autoStopOff();      // Make sure auto-stop is off
00018     innovationOff();    // make sure innovation (collision detection mode) is off
00019     
00020     // VARIABLES
00021     
00022     contactBatt = 1;
00023     
00024     // Auto-Stop Challenge
00025     autoStopActive = false;             // Flag is auto-stop mode is active
00026     autoStopInProgress = false;         // Flag if auto-stop track-side sensor has been detected and auto-stop is in progress
00027     autoStopCruiseSpeed = 0;            // Speed of loco at time of auto-stop trackside sensor triggering so loco can maintain this speed without operator input
00028     autoStopThrottle = 0;               // Throttle rate applied at the time of triggering autostop
00029     targetDistance = 25.00f;            // How far in meters to bring the loco to a stop
00030     remainingDistance = targetDistance; // How far the loco has left to go before reaching target distance
00031     decelerationGradient = 0;           // Gradient of y=mx+c linear speed-distance curve that is used in this auto-stop version
00032     requiredSpeed = 0;                  // How fast the loco should be going according to y=mx+c line
00033     
00034     // Innovation Challenge
00035     innovationDistanceLimit = 1500;     // Stopping distance in mm from IR sensors
00036     stopLoco = false;                   // Flag to stop the loco when obstacle has been detected
00037     
00038     // IR Output voltages at various distances (for calibration purposes)
00039     voltageAt5500 = 0;
00040     voltageAt5000 = 0;
00041     voltageAt4500 = 0;
00042     voltageAt4000 = 0;
00043     voltageAt3500 = 0;
00044     voltageAt3000 = 0;
00045     voltageAt2500 = 0;
00046     voltageAt2000 = 0;
00047     voltageAt1500 = 0;
00048     voltageAt1000 = 2.2f;
00049     voltageAt500  = 3.0f;
00050     
00051 }    // CONSTRUCTOR
00052 
00053 void ChallengeMode::regenThrottleOn() {
00054     // OPEN THE BATTERY CONTACTOR SO POWER IS DELIVERED BY SUPERCAPS
00055     
00056     contactCapCharge = 0;               // Open supercap charging contactor to prevent charging
00057     
00058     contactBatt = 0;                    // Open battery contactor so all power comes from supercaps
00059     regenThrottleActive = true;         // Flag to indicate that regen throttling is on
00060     pc.printf("Regen Throttle On\r\n");
00061 }
00062 
00063 void ChallengeMode::regenThrottleOff() {
00064     // CLOSE THE BATTERY CONTACTOR
00065     if (regenBrakingActive == false) {  // If regen braking is not active and using the supercaps, allow capacitor to pre-charge from batteries
00066         contactCapCharge = 1;           // Close the supercap charging contactor
00067     }
00068     
00069     contactBatt = 1;                    // Close the battery contactor so power comes from batteries
00070     regenThrottleActive = false;        // Flag to indicate that regen throttling is off
00071     pc.printf("Regen Throttle Off\r\n");
00072 }
00073 
00074 bool ChallengeMode::regenBrakingOn() {
00075     // TURN ON REGEN BRAKING
00076 //     if (superCapVoltage == 0) {             // If super caps are not full
00077 //      contactCapCharge = 0;               // Open the super cap pre-cahrging contactor
00078 //        contactBatt = 0;                    // Open battery contactor so all power comes from supercaps
00079         regenBrakingActive = true;          // Flag to indicate that regen throttling is on
00080         pc.printf("Regen Braking On\r\n");
00081         return 1;                           // Return 1 if regen braking switched on
00082 //    }
00083 //    else {
00084 //        return 0;                           // Return 0 is regen braking didnt turn on due to full supercaps
00085 //    }
00086 }
00087 
00088 void ChallengeMode::regenBrakingOff() {
00089     // TURN OFF REGEN BRAKING
00090     if (regenThrottleActive == false) {     // If regen throttle is not active and using the supercaps, allow capacitor to pre-charge from batteries
00091         contactCapCharge = 1;               // Close the supercap pre-charge contactor
00092     }
00093     regenBrakingActive = false;             // Flag that regen braking is off
00094     contactBatt = 1;                    // Close battery contactor so all power comes from supercaps
00095     pc.printf("Regen Braking Off\r\n");
00096 }
00097 
00098 void ChallengeMode::autoStopOn() {
00099     // TURN ON AUTOSTOP MODE
00100     autoStopActive = true;                  // Flag that auto-stop mode is on
00101     pc.printf("Auto-Stop On\r\n");
00102 }
00103 
00104 void ChallengeMode::autoStopTriggered() {
00105     // INTERRUPT FUNCTION CALLED WHEN TRACK-SIDE TRIGGER HAS BEEN DETECTED
00106     if (autoStopActive == true && autoStopInProgress == false) {    // If auto-stop mode is on and signal has not yet been detected
00107         autoStopInProgress = true;                                  // Flag that auto-stop is in progress and fully autonomous
00108         autoStopCruiseSpeed = _dashboard.currentSpeed;              // Set the speed the loco was going at the point of triggering
00109         autoStopThrottle = _remote.throttle;
00110         whistleValve32 = 1;
00111         _dashboard.currentDistance = 0.00f;                         // Reset the distance-travelled counter to 0
00112 //        timeToReachTarget = targetDistance / (autoStopCruiseSpeed * 1000 / 3600);   // Time(s) = Distance(m) / Speed(converted to m/s)
00113         decelerationGradient = (autoStopCruiseSpeed - 1) / (targetDistance - 1);    // m = y / x → to get to 1kph at 24m, leaving ~ 4 seconds to get to target of 25m
00114         pc.printf("Auto-Stop Triggered\r\n");
00115     }
00116 }
00117 
00118 void ChallengeMode::autoStopControl() {
00119     // FUNCTION TO MANAGE THE LOCO THROTTLE AND BRAKING WHEN AUTO-STOPPING
00120     remainingDistance = targetDistance - _dashboard.currentDistance;        // Calculate remaining distance from target distance
00121 //    timeToReachTarget = int(_dashboard.currentSpeed) > 0 ? remainingDistance / _dashboard.currentSpeed : 999;
00122     
00123     // FOLLOWING DECELERATION GRADIENT
00124     
00125     if (remainingDistance > 2.0f) {    // IF OVER 3M FROM TARGET, CONTROL SPEED (1m + 2m sensor to nose of train)
00126     
00127         _motor1.throttle(0.3f);     // Apply 30% throttle
00128         //requiredSpeed = (decelerationGradient * _dashboard.currentDistance) + autoStopCruiseSpeed;  // (y = mx + c)
00129 //        
00130 //        if (_dashboard.currentSpeed < requiredSpeed) {  // If train is below the speed it should be, apply 30% throttle and no braking
00131 ////            _motor1.throttle(0.3f);
00132 ////            _motor2.throttle(0.1f);
00133 ////            _motor1.brake(0.0f);
00134 ////            _motor2.brake(0.0f);
00135 //
00136 //            brakeValve32 = 1;
00137 //        }
00138 //        else {                                                              // If loco is going faster than is should be
00139 //            int speedDifference = _dashboard.currentSpeed - requiredSpeed;  // Work out how much faster it is going and apply a proportional amount of motor braking
00140 //            
00141 //            _motor1.throttle(0.0f);         // Turn off throttle
00142 ////            _motor2.throttle(0.0f);
00143 //
00144 //            brakeValve32 = 0;
00145 //            if (pressureSwitch1.read() == 0) {
00146 //                brakeValve22 = 0;
00147 //                pc.printf("Pressure 1 Reached");
00148 //            }
00149 //            
00150 //            switch (speedDifference) {  // MOTOR BRAKING
00151 //                case 1:
00152 //                    _motor1.brake(0.1f);
00153 ////                    _motor2.brake(0.1f);
00154 //                    break;
00155 //                    
00156 //                case 2:
00157 //                    _motor1.brake(0.2f);
00158 ////                    _motor2.brake(0.2f);
00159 //                    break;
00160 //                    
00161 //                case 3:
00162 //                    _motor1.brake(0.3f);
00163 ////                    _motor2.brake(0.3f);
00164 //                    break;
00165 //                    
00166 //                case 4:
00167 //                    _motor1.brake(0.4f);
00168 ////                    _motor2.brake(0.4f);
00169 //                    break;
00170 //                    
00171 //                case 5:
00172 //                    _motor1.brake(0.5f);
00173 ////                    _motor2.brake(0.5f);
00174 //                    break;
00175 //                    
00176 //                case 6:
00177 //                    _motor1.brake(0.6f);
00178 ////                    _motor2.brake(0.6f);
00179 //                    break;
00180 //                    
00181 //                case 7:
00182 //                    _motor1.brake(0.7f);
00183 ////                    _motor2.brake(0.7f);
00184 //                    break;
00185 //                    
00186 //                case 8:
00187 //                    _motor1.brake(0.8f);
00188 ////                    _motor2.brake(0.8f);
00189 //                    break;
00190 //                    
00191 //                case 9:
00192 //                    _motor1.brake(0.9f);
00193 ////                    _motor2.brake(0.9f);
00194 //                    break;
00195 //                    
00196 //                default:
00197 //                    _motor1.brake(1.0f);
00198 ////                    _motor2.brake(1.0f);
00199 //                    break;
00200 //            }   // switch
00201 //        }// else
00202     }// if 
00203     else {  // IF REACHED STOPPING TARGET AREA
00204         rtc_Trigger = 0;        // APPLY EMERGENCY BRAKES
00205 //        if (remainingDistance > 0.2f) {     // If more than 20cm to go, stay at 1kph
00206 //        
00207 //        }
00208 //        else {                              // If less than 20cm from target apply full mechanical brakes and turn off throttle
00209 //            _motor1.throttle(0.0f);
00210 ////            _motor2.throttle(0.0f);
00211 //            
00212 //            // MECHANICAL BRAKES
00213 //            brakeValve32 = 1;
00214 //            if (pressureSwitch3 == 0) {
00215 //                brakeValve22 = 0;
00216 //            }
00217 //            else {
00218 //                brakeValve22 = 1;   
00219 //            }
00220 //        }
00221     }
00222     
00223     pc.printf("remainingDistance  %f\r\n", remainingDistance);
00224 //    pc.printf("timeToReachTarget %f\r\n", timeToReachTarget);
00225 }
00226 
00227 void ChallengeMode::autoStopOff() {
00228     // TURN OFF AUTOSTOP MODE
00229     autoStopActive = false;         // CLEAR AUTOSTOP MODE FLAG
00230     autoStopInProgress = false;     // CLEAR AUTOSTOPPING
00231     rtc_Trigger = 1;
00232     pc.printf("Auto-Stop Off\r\n");
00233 }
00234 
00235 void ChallengeMode::innovationOn() {
00236     // TURN ON INNOVATION MODE
00237     innovationActive = true;
00238     stopLoco = false;
00239     rtc_Trigger = 1;
00240     _motor1.setSpeedMode(1);     // SET MOTORS TO INCH FORWARD MODE
00241 //    _motor2.setSpeedMode(1);
00242     pc.printf("Innovation On\r\n");
00243 }
00244 
00245 void ChallengeMode::innovationOff() {
00246     // TURN OFF INNOVATION MODE
00247     innovationActive = false;
00248     stopLoco = false;
00249     rtc_Trigger = 1;
00250     _motor1.setSpeedMode(3);     // SET MOTOR SPEED LIMIT TO MAX
00251     pc.printf("Innovation Off\r\n");
00252 }
00253 
00254 int ChallengeMode::innovationControl(int requestedThrottle) {
00255     // CONTROL THE TRAIN THROTTLING AND BRAKING
00256     
00257     int count = 0;
00258     
00259     if (irSensor_1 > voltageAt1000 / 3.3f) { count++; }
00260     if (irSensor_2 > voltageAt1000 / 3.3f) { count++; }
00261     if (irSensor_3 > voltageAt1000 / 3.3f ) { count++; }
00262     
00263     if (count >= 2) {
00264         stopLoco = true;
00265     }
00266 
00267     
00268     if (stopLoco == true) { // IF SENSORS INDICATE TRAIN SHOULD STOP
00269     
00270 //        // APPLY MECHANICAL BRAKES
00271 //        brakeValve32 = 0;
00272 //        brakeValve22 = 1;
00273 
00274         // APPLY E-BRAKE
00275         rtc_Trigger = 0;
00276         
00277         pc.printf("Obstacle Detected\r\n");
00278         
00279         return 0;       // RETURN THROTTLE = 0 TO INDICATE TRAIN SHOULD STOP
00280     }
00281     else {
00282         return requestedThrottle;   // OTHERWISE THROTTLE = USER REQUEST
00283     }
00284 }