main.cpp

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