main.cpp

00001 #include "mbed.h"
00002 #include <iostream>
00003 #include <math.h> 
00004 #define TI  .001f       //1kHz sample time
00005 #define dKP  5.1491f     //Proportional Gain
00006 #define dKI  119.3089f   //Integral Gain
00007 #define FPWM .000034f    //PWM Frequency#define TI 0.001 //1kHz sample time
00008 #define sKP 5.375f//5.50f //Proportional Gain
00009 #define sKD 0.003f //Derivative Gain was .01
00010 #define sKI 0.025f//0.001f//0.0001f //Derivative Gain was .01
00011 #define ssKP 1.0f
00012 #define ssKD 0.001f
00013 #define ssKI 0.001f
00014 #define DCINTERCEPT 0.06f //duty cycle intercept was 0.061
00015 #define DCSLOPE 0.03f //duty cycle slope was .05
00016 #define TOL 0.005f               //tolerance
00017 #define LEDOFF 0.0f
00018 #define LEDON 1.0f
00019 #define RATIO 3.9625f
00020 #ifndef M_PI
00021 #define M_PI 3.14f
00022 #endif
00023 #define MIN -40.0*(M_PI/180)
00024 #define MAX -40.0*(M_PI/180)
00025 
00026 #define BLUETOOTHBAUDRATE 115200 //Communication rate of the micro-controller
00027                                  //to the Bluetooth module
00028 #define SERIALTXPORT PTE0 //Tx Pin (Sends information to serial device)
00029 #define SERIALRXPORT PTE1 //Rx Pin (Receives information from serial
00030 Serial bt(SERIALTXPORT, SERIALRXPORT); //(TX, RX) Serial 
00031 //Pins
00032 PwmOut led_red(LED1);
00033 PwmOut led_blue(LED3);
00034 PwmOut led_green(LED2);
00035 
00036 AnalogIn _SpeedReference(A0);
00037 AnalogIn _Tacho(A1);
00038 PwmOut _MotorControl(PTD0);
00039 DigitalOut _BreakSignal(PTC9);//D5
00040 
00041 AnalogIn _LeftSensors(A2);
00042 AnalogIn _RightSensors(A3);
00043 PwmOut _servo(PTE20);//A13
00044 
00045 //Binary LED
00046 DigitalOut _FirstBinaryLED(A5);
00047 DigitalOut _SecondBinaryLED(PTE21);
00048 DigitalOut _ThirdBinaryLED(PTD2);
00049 DigitalOut _FourthBinaryLED(PTE31);
00050 //Hardware Interupts
00051 Ticker Update;
00052 Ticker sUpdate;
00053 Ticker loopUpdate;
00054 DigitalIn _RunButton(PTA4);
00055 DigitalIn _StopButton(PTA5);
00056 DigitalIn _BumperStop(PTD3);
00057 AnalogIn _BattMon(A4);
00058 
00059 InterruptIn arrowheadLeft(PTD5);//PTA1
00060 InterruptIn arrowheadRight(PTA13);//PTA2
00061 
00062 
00063 //Integral Value 
00064 float _errorArea;
00065 float _error;
00066 float _controllerOutput;
00067 float _steeringArea;
00068 float _ref;
00069 float _feedbackPrev;
00070 int _counter = 0;
00071 bool _ArrowHitRight;
00072 bool _ArrowHitLeft;
00073 float _steeringControllerOut;
00074 float _dutyCycle;
00075 
00076 
00077 
00078 //States
00079 enum States {RUN, WAIT, STOP, EMPTY};
00080 States _NextState;
00081 States _CurrentState;
00082 
00083 //Methods
00084 void Init();
00085 void Loop();
00086 void UpdateMethod();
00087 void UpdateDriveControl();
00088 void UpdateSteeringControl();
00089 void UpdateLowBatteryStop();
00090 void UpdateLoop();
00091 void SetMotor(float);
00092 void BreaksOn();
00093 void BreaksOff();
00094 void StopMethod();
00095 void WaitMethod();
00096 void RunMethod();
00097 void RunButtonMethod();
00098 void StopButtonMethod();
00099 void BumperStopMethod();
00100 void UpdateLeft();
00101 void UpdateRight();
00102 void UpdateBinaryCounter();
00103 
00104 void UpdateSteeringControlStraight();
00105 
00106 //void PrintStatus();
00107 
00108 //State Methods
00109 void TriggerSwitch();
00110 void StopMethod();
00111 void WaitMethod();
00112 void RunMethod();
00113 
00114 int main() {
00115     cout << "\rCarStart" << endl;
00116     Init();
00117     while(1) {Loop();}
00118     //int r = counter; for(int b = 3; b <= 0; b--){ if(r > pow(2,b+1)){ LED[b] = LEDON; r = r - pow(2,b+1);}           
00119 }
00120 
00121 void Init() //Initializer
00122 {
00123     bt.baud(BLUETOOTHBAUDRATE);
00124     _MotorControl.period(FPWM);
00125     SetMotor(0.0f); //Turns off the motor
00126     _error = 0.0f;
00127     _errorArea = 0.0f;
00128     _ref = 0.0f;
00129     _steeringControllerOut = 0.0f;
00130     _dutyCycle= 0.0f;
00131     _steeringArea= 0.0f;
00132     _NextState = EMPTY;
00133     _CurrentState = EMPTY;
00134     StopMethod();
00135     TriggerSwitch();
00136     _feedbackPrev = 0.0f;
00137     _servo.period(0.02);
00138    // _RunButton.rise(&TriggerSwitch);
00139    // _StopButton.rise(&StopMethod);
00140     //_BumperStop.rise(&BumperStopMethod);
00141     arrowheadLeft.rise(&UpdateLeft);
00142     arrowheadRight.rise(&UpdateRight);
00143     BreaksOn();
00144     _ArrowHitRight = false;
00145     _ArrowHitLeft = false;
00146     Update.attach(&UpdateDriveControl, TI);
00147     //Update.attach(&UpdateMethod, TI);
00148     sUpdate.attach(&UpdateSteeringControl, TI);
00149     loopUpdate.attach(&UpdateLoop, 0.75f
00150     );
00151     //SetMotor(_ref);
00152 }
00153 
00154 void Loop() //Repeated Loop
00155 {
00156     /*wait(.75);
00157     
00158     if(_StopButton.read()>=.9)
00159         StopMethod();
00160     if(_RunButton.read()>=.9)
00161         TriggerSwitch();
00162     
00163     _ArrowHitRight = false;
00164     _ArrowHitLeft = false;
00165     UpdateLowBatteryStop();
00166     led_blue = !led_blue;
00167     //PrintStatus();
00168     //bt.printf("\rGroup 2: USC Football team\n Josh: %f      \033[F",_LeftSensors.read() - _RightSensors.read());
00169     //_counter++;
00170     //if(_counter == 11) _counter = 0; 
00171     //UpdateBinaryCounter();
00172     cout<< "\r"<<_SecondBinaryLED.read() << endl;
00173     //cout<< "\r"<<_controllerOutput << endl;*/
00174 }
00175 
00176 void UpdateLoop() //
00177 {
00178     if(_StopButton.read()>=.9)
00179         StopMethod();
00180     if(_RunButton.read()>=.9)
00181         TriggerSwitch();
00182     if(_BumperStop.read()<=.3)
00183         StopMethod();
00184     _ArrowHitRight = false;
00185     _ArrowHitLeft = false;
00186     UpdateLowBatteryStop();
00187     led_blue = !led_blue;
00188     UpdateLowBatteryStop();
00189     bt.printf("\rGroup 2: USC Football team\n Josh: %f      \033[F",_LeftSensors.read() - _RightSensors.read());
00190 } 
00191 void UpdateMethod() //
00192 {
00193     UpdateDriveControl();
00194     UpdateSteeringControl();
00195     //UpdateLowBatteryStop();
00196 } 
00197     
00198 void UpdateDriveControl()
00199 {
00200     if(_CurrentState == RUN)
00201     {
00202     _ref = 0.6; //_SpeedReference.read();
00203     _error = _ref - _Tacho.read();
00204     _errorArea += TI*_error;
00205     _controllerOutput = dKP*_error+ dKI*_errorArea;
00206     }
00207     SetMotor(_controllerOutput);
00208    
00209 }
00210 
00211 void UpdateSteeringControl(void)
00212 {
00213     if(_CurrentState != STOP){
00214     float feedback = _LeftSensors.read() - _RightSensors.read();
00215     float reference = 0.075f;
00216     float err = reference - feedback;
00217     float feedbackChange = (feedback - _feedbackPrev)/TI;
00218     _steeringArea += TI*err;
00219     _steeringControllerOut = sKP*err + sKD*feedbackChange + sKI*_steeringArea;
00220     float controllerOutNorm = (_steeringControllerOut + M_PI/2.0f)/M_PI;
00221     _dutyCycle = DCSLOPE*controllerOutNorm + DCINTERCEPT;
00222     
00223     /*if(_dutyCycle >= .085)
00224         _dutyCycle = .085;
00225     if(_dutyCycle <= .065)
00226         _dutyCycle = .065;*/
00227     if (abs(_dutyCycle-_servo.read()) > TOL)
00228         _servo.write(_dutyCycle);
00229         
00230     _feedbackPrev = feedback;
00231     
00232     if (_LeftSensors.read() < .4091 && _RightSensors.read() < .40152){
00233         StopMethod();
00234         //_CurrentState = STOP;
00235         //BreaksOn();
00236         //led_red =0.25f;
00237         //led_blue =0.25f;
00238         //led_green =0.25f;
00239         //_NextState = WAIT;
00240         }
00241     }
00242     
00243 }
00244 
00245 
00246 
00247 void UpdateLowBatteryStop()
00248 {
00249     float val = 0.5;       // variable for the A/D value
00250     float pinVoltage = 0; // variable to hold the calculated voltage
00251     float batteryVoltage = 0; 
00252     val = _BattMon.read();    // read the voltage on the divider  
00253   
00254     pinVoltage = val * 0.30303;       //  Calculate the voltage on the A/D pin
00255                                     //  A reading of 1 for the A/D = 0.0048mV
00256                                     //  if we multiply the A/D reading by 0.00488 then 
00257                                     //  we get the voltage on the pin.                                  
00258                                     
00259     batteryVoltage = pinVoltage * RATIO;    //  Use the ratio calculated for the voltage divider
00260                                           //  to calculate the battery voltage
00261     if(batteryVoltage < 1.00){
00262       cout << "\n\rBattery Voltage is too low. Stop Method " << endl;
00263       StopMethod();
00264       } else{
00265           cout << "\n\rBattery Voltage is: " << batteryVoltage << endl;
00266           }
00267   
00268     //wait(1000);                 //  Slow it down    
00269 }
00270 
00271 void SetMotor(float speed)
00272 {
00273     float invD = 0.0f;
00274     if(speed<0.0f)
00275     invD =0.0f;
00276     else if(speed> 0.9f)
00277     invD = 0.9f;
00278     else
00279     invD = speed;   
00280  
00281     _MotorControl.write(/*1.0f-*/invD);
00282 }
00283 
00284 void BreaksOn()
00285 {
00286     _controllerOutput = 0;
00287     _errorArea = 0;
00288     _steeringArea = 0.0;
00289     _BreakSignal = 1;
00290     _servo=0.1f;
00291 }
00292 
00293 void BreaksOff()
00294 {
00295     _controllerOutput = 0;
00296     _steeringArea = 0.0;
00297     _errorArea = 0;
00298     _BreakSignal = 0;
00299 }
00300 
00301 void WaitMethod()
00302 {
00303     _CurrentState = WAIT;
00304     BreaksOff();
00305     led_red =.125;
00306     led_blue =1;
00307     led_green =.125;
00308     _NextState = RUN;
00309 }
00310 
00311 void RunMethod()
00312 {
00313     _CurrentState = RUN;
00314     BreaksOff();
00315     led_red =1;
00316     led_blue =1;
00317     led_green =0.25;
00318     _NextState = WAIT;
00319 }
00320 
00321 void StopMethod()
00322 {   
00323     _CurrentState = STOP;
00324     BreaksOn();
00325     led_red =0.25;
00326     led_blue =1;
00327     led_green =1;
00328     _NextState = WAIT;
00329 }
00330 
00331 void BumperStopMethod()
00332 {   
00333     _CurrentState = STOP;
00334     BreaksOn();
00335     led_red =0.25;
00336     led_blue =1;
00337     led_green =1;
00338     _NextState = WAIT;
00339 }
00340 
00341 void LowBatteryStopMethod()
00342 {
00343     _CurrentState = STOP;
00344     BreaksOn();
00345     led_red=0.25;
00346     led_blue=1;
00347     led_green=1;
00348     _NextState = WAIT;
00349 }
00350 
00351 void TriggerSwitch()
00352 {
00353     switch(_NextState){
00354     case STOP:
00355     StopMethod();
00356     break;
00357     
00358     case  WAIT:
00359     WaitMethod();
00360     break;
00361     
00362     case RUN:
00363     RunMethod();
00364     break;
00365     
00366     case EMPTY:
00367     led_red =0.25;
00368     led_blue =0.25;
00369     led_green =0.25;
00370     _NextState = WAIT;
00371     break;
00372     }
00373 }
00374      
00375 void UpdateLeft(void)
00376 {
00377  
00378     if(_counter >=10) {
00379         _counter = 0;
00380     } else {
00381         _counter += 1;
00382     }
00383     if(_ArrowHitRight)
00384     {_counter = 0;_ArrowHitRight=!_ArrowHitRight;
00385     _steeringArea =0;}
00386     cout << "\n\rcounter =  " << _counter << endl;
00387     cout << "\n\rLEFT" << endl;
00388     UpdateBinaryCounter();
00389     _ArrowHitLeft = true;
00390     //if(led_blue == 1.0f)led_blue = 0.0f;
00391     //else led_blue = 1.0f;
00392 }
00393     
00394 void UpdateRight(void)
00395 {
00396  
00397     if(_counter >=10) {
00398         _counter = 0;
00399     } else {
00400         _counter += 1;
00401     }
00402     if(_ArrowHitLeft)
00403     {_counter = 0;_ArrowHitLeft=!_ArrowHitLeft;
00404     _steeringArea = 0;}
00405     cout << "\n\rcounter =  " << _counter << endl;
00406     cout << "\n\rRIGHT" << endl;
00407     UpdateBinaryCounter();
00408     _ArrowHitRight = true;
00409     //if(led_blue == 1.0f)led_blue = 0.0f;
00410     //else led_blue = 1.0f;
00411 }
00412 
00413 void UpdateBinaryCounter()
00414 {
00415     switch(_counter){
00416         case 0:
00417             _FirstBinaryLED = LEDOFF;
00418             _SecondBinaryLED = LEDOFF;
00419             _ThirdBinaryLED = LEDOFF;
00420             _FourthBinaryLED = LEDOFF;
00421         break;
00422         case 1:
00423             _FirstBinaryLED = LEDON;
00424             _SecondBinaryLED = LEDOFF;
00425             _ThirdBinaryLED = LEDOFF;
00426             _FourthBinaryLED = LEDOFF;
00427         break; 
00428         case 2:
00429             _FirstBinaryLED = LEDOFF;
00430             _SecondBinaryLED = LEDON;
00431             _ThirdBinaryLED = LEDOFF;
00432             _FourthBinaryLED = LEDOFF;
00433         break;
00434         case 3:
00435             _FirstBinaryLED = LEDON;
00436             _SecondBinaryLED = LEDON;
00437             _ThirdBinaryLED = LEDOFF;
00438             _FourthBinaryLED = LEDOFF;
00439         break;    
00440         case 4:
00441             _FirstBinaryLED = LEDOFF;
00442             _SecondBinaryLED = LEDOFF;
00443             _ThirdBinaryLED = LEDON;
00444             _FourthBinaryLED = LEDOFF;
00445         break;
00446         case 5:
00447             _FirstBinaryLED = LEDON;
00448             _SecondBinaryLED = LEDOFF;
00449             _ThirdBinaryLED = LEDON;
00450             _FourthBinaryLED = LEDOFF;
00451         break;    
00452         case 6:
00453             _FirstBinaryLED = LEDOFF;
00454             _SecondBinaryLED = LEDON;
00455             _ThirdBinaryLED = LEDON;
00456             _FourthBinaryLED = LEDOFF;
00457         break;
00458         case 7:
00459             _FirstBinaryLED = LEDON;
00460             _SecondBinaryLED = LEDON;
00461             _ThirdBinaryLED = LEDON;
00462             _FourthBinaryLED = LEDOFF;
00463         break;
00464         case 8:
00465             _FirstBinaryLED = LEDOFF;
00466             _SecondBinaryLED = LEDOFF;
00467             _ThirdBinaryLED = LEDOFF;
00468             _FourthBinaryLED = LEDON;
00469         break;
00470         case 9:
00471             _FirstBinaryLED = LEDON;
00472             _SecondBinaryLED = LEDOFF;
00473             _ThirdBinaryLED = LEDOFF;
00474             _FourthBinaryLED = LEDON;
00475         break;
00476         case 10:
00477             _FirstBinaryLED = LEDOFF;
00478             _SecondBinaryLED = LEDON;
00479             _ThirdBinaryLED = LEDOFF;
00480             _FourthBinaryLED = LEDON;
00481         break;
00482         default:
00483             _FirstBinaryLED = LEDOFF;
00484             _SecondBinaryLED = LEDOFF;
00485             _ThirdBinaryLED = LEDOFF;
00486             _FourthBinaryLED = LEDOFF;
00487         break;               
00488     } 
00489 }
00490 /*
00491 void PrintStatus()
00492 {
00493     if(bt.readable()) {
00494         char input = bt.getc();
00495         if(input == 'i')_CurrentState = RUN;
00496         if(input == 'o')_CurrentState = WAIT;
00497         if(input == 'p')_CurrentState = STOP;
00498         if(input == 'q')sKP+=.005;
00499         if(input == 'a')sKP-=.005;
00500         if(input == 'w')sKD+=.0001;
00501         if(input == 's')sKD-=.0001;
00502         if(input == 'e')sKI+=.001;
00503         if(input == 'd')sKI-=.001;
00504         if(input == 'r')_counter+=1;
00505         if(input == 'f')_counter-=1;
00506         
00507     }      
00508     bt.printf("\033[20F");    
00509     bt.printf("\r\n KP : %f",sKP);
00510     bt.printf("\r\n KD : %f",sKD);
00511     bt.printf("\r\n KI : %f",sKI);
00512     bt.printf("\r\n location : %d", _counter);
00513     bt.printf("\r\n Duty Cycle : %f", _dutyCycle);
00514     
00515 }*/