main.cpp

00001 #include <mbed.h>
00002 #include "millis.h"
00003 #include "brakes.h"
00004 #include "definitions.h"
00005 #include "remoteControl.h"
00006 #include "dashboard.h"
00007 #include "rtc.h"
00008 #include "motor.h"
00009 #include "challenge.h"
00010 #include <sstream>
00011 using std::string;
00012 
00013 //  SET UP REMOTE CONTROL COMMS
00014 SPI remoteControl(PE_14, PE_13, PE_12);   // (SPI_MOSI, SPI_MISO, SPI_SCK)
00015 DigitalOut remoteControlCS(PE_11);    // (SPI_SS)
00016 
00017 //  CREATE OBJECTS
00018 Remote remote(remoteControl, remoteControlCS);
00019 Dashboard dashboard(hallSensor);
00020 RoundTrainCircuit rtc(rtc_1, rtc_2, rtc_3, rtc_4, rtc_5, rtc_6, rtc_7, rtc_override);
00021 Motor motor1(motorAccelerator, motorBrake, keySwitchM1, directionFwd, directionRev, footswitchM1, seatM1, inchFwdM1, speedLimit2M1, speedLimit3M1);
00022 ChallengeMode challenge(autoStopTrigger, dashboard, remote, motor1);
00023 Brakes brakes;
00024 
00025 int driveMode = 2;// Drive mode - fwd(0), rev(1), park(2)
00026 bool emergencyStopActive = false; //default state for emergency stop being active
00027 
00028 ////////FUNCTIONS
00029 
00030 //Display Function for data logger
00031 void DisplaySerial(){ 
00032     std::stringstream displayline;
00033     displayline << "Blackbox# " << " Motor Accelerator: " << motorAccelerator << " Brake 3/2: " << brakeValve32 <<  " Brake 2/2: " << brakeValve22 <<  " Speed: " << dashboard.currentSpeed << " Distance: " << dashboard.currentDistance << " Drive Mode: " << driveMode << "\n";
00034     string disp = displayline.str();
00035     pc.printf("%s \n", disp.c_str());
00036     }
00037 
00038 //Brake code 
00039 void brakeControl(int brakeRate) { //set brake rate to float for regen
00040   if (driveMode == 2) {   // PARK MODE
00041     //  All Mechanical brakes applied
00042       brakes.ParkMode(motor1);
00043   }
00044   {//REGEN BRAKING
00045   
00046   if (challenge.regenBrakingActive == true) { // REGEN BRAKING WITH OVERVOLTAGE SAFETY CHECK
00047        brakes.RegenControl(brakeRate,motor1);
00048       }
00049     
00050     else {  // MECHANICAL BRAKING
00051      brakes.MechanicalBraking(brakeRate,motor1); // calls mechanical braking 
00052     }
00053   }
00054   return;
00055 }
00056 void emergencyStop()
00057 {
00058     brakes.EmergencyStop(motor1,rtc,emergencyStopActive); //invokes emergency stop code from brakes class 
00059 }
00060 //Motor code
00061 void speedControl(int commandedSpeed) {
00062   switch (commandedSpeed) {
00063 
00064     default:
00065       break;
00066 
00067     case 0:
00068       motor1.throttle(0.0f);
00069       break;
00070 
00071     case 1 ... 2:
00072       motor1.throttle(0.1f);
00073       break;
00074 
00075     case 3 ... 4:
00076       motor1.throttle(0.2f);
00077       break;
00078 
00079     case 5 ... 6:
00080       motor1.throttle(0.3f);
00081       break;
00082 
00083     case 7 ... 8:
00084       //motor1.throttle(0.4f);
00085       motor1.throttle(0.3f);
00086       break;
00087 
00088     case 9 ... 10:
00089       //motor1.throttle(0.5f);
00090       motor1.throttle(0.3f);
00091       break;
00092 
00093     case 11:
00094       //motor1.throttle(0.6f);
00095       motor1.throttle(0.3f);
00096       break;
00097 
00098     case 12:
00099       //motor1.throttle(0.7f);
00100       motor1.throttle(0.3f);
00101       break;
00102 
00103     case 13:
00104       //motor1.throttle(0.8f);
00105       motor1.throttle(0.3f);
00106       break;
00107 
00108     case 14:
00109       //motor1.throttle(0.9f);
00110       motor1.throttle(0.3f);
00111       break;
00112 
00113     case 15:
00114       //motor1.throttle(1.0f);
00115       motor1.throttle(0.3f);
00116       
00117       break;
00118   }
00119 }
00120 ///Energy Storage
00121 //energy storage display code
00122 void EnergyStorage()
00123 {
00124     float current_powercab=((2*vout_powercab)-vref_powercab)*250; //voltage change to current conversion
00125     float current_supercap=((2*vout_supercap)-vref_supercap)*250;
00126     int t=1; //1 second interval
00127     int C= 250; //Capacitance Value
00128     float energy_supercap = (1/2) * (current_supercap * current_supercap) * (t*t) / C;
00129     float energy_powercab = (1/2) * (current_powercab * current_powercab) * (t*t) / C;
00130     int scap = static_cast<int>(energy_supercap);
00131     int pcab = static_cast<int>(energy_powercab);
00132     remote.sendData(2,scap);
00133     remote.sendData(2,pcab);
00134     
00135 }
00136 
00137 
00138 int main() {
00139   pc.baud(115200);
00140 
00141   // CONFIGURE INTERRUPTS
00142   rtc_output.rise(&emergencyStop);
00143 
00144   millisStart();
00145 
00146   rtc_Trigger = 1;
00147 
00148   // LOCAL VARIABLES
00149   bool systemOn = false;   // On/Off status of loco
00150   int lastKnownDirection = 2;
00151   bool inParkMode = false;
00152 
00153   // Record last time error was sent
00154   unsigned long lastErrorMillis = 0;
00155 
00156   //MainLoop
00157   while (1) {
00158 
00159     while (remote.commsGood == true) {
00160       /////Start Up///////////////////////////////////////////////////////////////////////////////////////////////////////////
00161 
00162       /////Checking Modes from controller/////////////////////////////////////////////////////////////////////////////////
00163       // PING
00164       remote.commsCheck();
00165 
00166       // GET SWITCH STATES
00167       remote.getSwitchStates();
00168 
00169       // ALLOW BRAKES TO BE OPERATED
00170       brakeControl(remote.braking);
00171       
00172       //Energy Storage Display
00173       EnergyStorage();
00174 
00175       // SOUND WHISTLE IF WHISTLE BUTTON PRESSED
00176       if (remote.whistle == 0) {
00177         whistleValve32 = 1;
00178       }
00179       else {
00180         whistleValve32 = 0;
00181       }
00182 
00183       //  GET AND DISPLAY SPEED
00184       dashboard.getCurrentSpeed();
00185       remote.sendData(2, dashboard.currentSpeed);       // Send speed to remote
00186 
00187       // TOGGLE COMPRESSOR
00188       remote.compressor == 0 ? contactCompressor = 1 : contactCompressor = 0;
00189 
00190       // TOGGLE MOTOR CONTROLLER DEADMAN (SEAT SWITCH AND FOOTBRAKE)
00191       if (rtc.deadman == 0) { // IF DEADMAN PRESSED
00192         motor1.closeDeadman();
00193       }
00194       else {
00195         motor1.releaseDeadman();
00196       }
00197 
00198       // TOGGLE REGEN THROTTLING
00199       if (challenge.regenThrottleActive == false) {
00200         if (remote.regenThrottle == 0 && remote.start == 0) {    // TURN OFF IF ON
00201           challenge.regenThrottleOn();
00202         }
00203       }
00204       else {
00205         remote.sendError('B');     // Send error to remote
00206         if (remote.regenThrottle == 1) {    // TURN ON IF OFF
00207           challenge.regenThrottleOff();
00208         }
00209       }
00210 
00211       //  TOGGLE REGEN BRAKING
00212       if (challenge.regenBrakingActive == false) {
00213         if (remote.regenBrake == 0 && remote.start == 0) {    // TURN OFF IF ON
00214           if (challenge.regenBrakingOn() == 0) {
00215             remote.sendError('I');     // Send error to remote
00216             pc.printf("Regen Braking Off - SuperCaps are full\r\n");
00217           }
00218         }
00219       }
00220       else {
00221         remote.sendError('C');     // Send error to remote
00222         if (remote.regenBrake == 1) {     // TURN OFF
00223           challenge.regenBrakingOff();
00224           if (superCapVoltage == 1) {
00225             pc.printf("Regen Braking Off - SuperCaps are full\r\n");
00226             remote.sendError('I');     // Send error to remote
00227           }
00228         }
00229       }
00230 
00231       //  TOGGLE AUTOSTOP
00232       if (challenge.autoStopActive == 0) {
00233         if (remote.autoStop == 0 && remote.start == 0) {    // TURN OFF IF ON
00234           challenge.autoStopOn();
00235         }
00236       }
00237       else {
00238         remote.sendError('D');     // Send error to remote
00239         if (remote.autoStop == 1) {    // TURN ON IF OFF
00240           challenge.autoStopOff();
00241         }
00242       }
00243 
00244       //  TOGGLE INNOVATION
00245       if (challenge.innovationActive == 0) {
00246         if (remote.innovation == 0 && remote.start == 0) {    // TURN OFF IF ON
00247           if (driveMode == 0) {
00248             challenge.innovationOn();
00249           }
00250           else {
00251             remote.sendError('J');     // Send error to remote
00252             pc.printf("Can only active innovation mode in forward direction\r\n");
00253           }
00254         }
00255       }
00256       else {
00257         remote.sendError('E');     // Send error to remote
00258 
00259         if (remote.innovation == 1) {    // TURN ON IF OFF
00260           challenge.innovationOff();
00261         }
00262       }
00263       /////END OF TOGGLE CHECKS//////////////////////////////////////////////////////////////////////////////////////////////
00264 
00265       /////Control///////////////////////////////////////////////////////////////////////////////////////////////////////////
00266       //Is a Toggle check, but it is the train start swtich A
00267       if (systemOn == false) {
00268         if (remote.start == 1) {
00269           if (millis() - lastErrorMillis > 500) {
00270             remote.sendError('A');   // SEND ERROR MESSAGE 'A' TO REMOTE
00271             lastErrorMillis = millis();
00272           }
00273           motor1.turnOff();
00274         }
00275         else {
00276           systemOn = true;
00277           motor1.turnOn();    // Turn on motors
00278         }
00279       }   // END IF SYSTEMON = FALSE
00280       //If train is switched on and in start do this start///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00281       else {  // IF SYSTEMON == TRUE
00282         if (remote.start == 1) {
00283           systemOn = false;       // WILL STOP ABOVE HERE NEXT LOOP
00284           pc.printf("Start Switch is Off\r\n");
00285         }
00286 
00287         //Set foward
00288         if (driveMode != 0 && remote.forward == 0) {
00289           driveMode = 0;
00290           motor1.setForward();
00291           FrontLight=1;
00292           BackLight=0;
00293         }
00294         //Set reverse
00295         if (driveMode != 1 && remote.reverse == 0) {
00296           driveMode = 1;
00297           motor1.setReverse();
00298           BackLight=1;
00299           FrontLight=0;
00300         }
00301         //Set park
00302         if (driveMode != 2 && remote.park == 0) {
00303           driveMode = 2;
00304           motor1.setPark();
00305           motor1.throttle(0);
00306         }
00307         ////Park Mode
00308         if (driveMode == 2) {     
00309              brakes.ParkMode(motor1);                        //place in park mode if selected by driver
00310           BackLight=0;
00311           FrontLight=0;
00312           if (inParkMode == false) {
00313             pc.printf("Train in park mode.\r\n"); //why?
00314           }
00315 
00316           if (emergencyStopActive == true && rtc_output.read() == 0) {   // Clear emergency stop flag
00317             emergencyStopActive = false;
00318           }
00319 
00320           led_parkMode = 1;
00321           inParkMode = true;      // Stop above debug print from displaying more than once
00322           motor1.setPark();
00323         }
00324 
00325         ////Drive
00326         else {                                                  //else i.e if selected drive mode is forward or reverse
00327           ////////////////// Start of check for error G RTC clear
00328           if (emergencyStopActive == false && rtc_output.read() == 0) {                                  // IF RTC FLAGGED AS SAFE
00329 
00330             led_parkMode = 0;
00331             inParkMode = false;
00332 
00333             if (driveMode != lastKnownDirection) {
00334               pc.printf("Train enabled for direction %d\r\n", driveMode);
00335 
00336               lastKnownDirection = driveMode;
00337             }
00338 
00339             ////Call autostop challenge
00340             if (challenge.autoStopInProgress == true) { // IF AUTOSTOPPING, PASS THROTTLE CONTROL TO FUNCTION
00341               challenge.autoStopControl();
00342               pc.printf("Autostop in Control\r\n");
00343             }
00344             //Use controls from remote
00345             else {  // OTHERWISE INPUT THROTTLE FROM REMOTE
00346               if (remote.throttle > 0) {   // If joystick pushed upwards = throttle
00347                 /////////////////////////Innovation braking start
00348                 if (challenge.innovationActive == true) {
00349                   pc.printf("Collision Detection in Control\r\n");
00350                   int innovationThrottle = challenge.innovationControl(remote.throttle);
00351                   speedControl(innovationThrottle);
00352 
00353                   if (innovationThrottle == 0) {
00354                       emergencyStop(); // emergency Brake if obstacle detected
00355                   }
00356                 }
00357                 /////////////////////////Innovation braking end
00358                 //normal throttle control
00359                 else {
00360                   speedControl(remote.throttle);
00361                   pc.printf("Throttling: %d\r\n", remote.throttle);
00362                 }
00363               } // remote.throttle
00364               ///if nothing set to 0
00365               else {
00366                 speedControl(0);
00367               }
00368             }
00369 
00370           }
00371           ////////////////// End of check for error G RTC clear
00372           else {
00373             pc.printf("Cannot exit park mode until RTC is cleared\r\n");
00374             inParkMode = false;
00375             remote.sendError('G');     // Send error to remote
00376           }
00377         }
00378         //Datalogger Chai Funciton
00379         DisplaySerial();
00380       }   // END IF (SYSTEMON == TRUE)
00381       //If train is switched on and in start do this end///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00382 
00383       wait_ms(25);   // SLOW DOWN THE SYSTEM (REMOTE CANT KEEP UP)
00384     }   // END WHILE(COMMSGOOD)
00385     pc.printf("Main Loop Skipped Due To Emergency Status\r\n");
00386     emergencyStop();    // Emergency stop if comms lost with remote controller
00387 
00388 
00389   }   //END WHILE(1)
00390 }