Railway Challenge
/
challenge
Uncommenting of part that allow supercaps to charge up from the batteries
Diff: challenge.cpp
- Revision:
- 0:4788e1df7b55
- Child:
- 1:ba85dae98035
--- /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