Railway Challenge
/
challenge
Uncommenting of part that allow supercaps to charge up from the batteries
challenge.cpp
- Committer:
- jamesmcildowietfl
- Date:
- 2022-04-27
- Revision:
- 15:4976d145fbd9
- Parent:
- 13:a9793222af20
- Child:
- 18:d28d458824d4
File content as of revision 15:4976d145fbd9:
#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; whistleValve32 = 1; _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 } }