Diff: main.cpp

Revision:

4:11559b04c4ff

Parent:

3:b17ae88bfa54

Child:

5:30cf394eca48

--- a/main.cpp	Wed Mar 27 17:24:22 2019 +0000
+++ b/main.cpp	Thu Apr 04 20:54:00 2019 +0000
@@ -1,85 +1,174 @@
 #include "mbed.h"
 #include <iostream>
-#define TI  .001f       //1kHz sample time
-#define dKP  5.1491f     //Proportional Gain
-#define dKI  119.3089f   //Integral Gain
-#define FPWM .00004f    //PWM Frequency#define TI 0.001 //1kHz sample time
-#define sKP 3.50f //Proportional Gain
-#define sKD 0.000f //Derivative Gain was .01
-#define DCINTERCEPT 0.061f //duty cycle intercept
-#define DCSLOPE 0.05f //duty cycle slope
-#define TOL 0.005f               //tolerance
+#include <math.h> 
+//General
+#define TI  .001f           //1kHz sample time
+#define LEDOFF 0.0f         //LED low value
+#define LEDON 1.0f          //LED high value
+#define BATTRATIO 3.9625f   //Battery Input ratio
+#define BATTCUTOFF 1.0f     //Battery Cutoff threshold
+#define LOOPTIME 0.75f      //Time(s) between loops
+#define ADCRATIO 0.30303f   //Ratio from port input to ADC value
+#define ZERO 0.0f           //Zero
 #ifndef M_PI
-#define M_PI 3.14f
+#define M_PI 3.14f          //Defines Pi
 #endif
 
+//Driving
+#define dKP  5.1491f        //Proportional Gain
+#define dKI  119.3089f      //Integral Gain
+#define MOTORPWM 0.00004f   //PWM Frequency
+#define MOTORMAX 0.6f       //Max Value of Buck Converter
+#define MOTORMIN 0.0f       //Minimum Value of Buck Converter
+
+//Steering
+#define SERVOPWM 0.02f      //Servo Frequency
+#define sKP 3.5f            //Proportional Gain
+#define sKD 0.000f          //Derivative Gain was .01
+#define DCINTERCEPT 0.061f  //Duty cycle intercept was 0.061
+#define DCSLOPE 0.05f       //Duty cycle slope
+#define TOL 0.005f          //Tolerance
+#define FULLLEFT 0.1f       //PWM value of servo turning left
+#define NLANDMARKS 10       //Number of Landmarks
+#define OFFTRACKRIGHT 0.401f//Right Off Track Threshold was .40152
+#define OFFTRACKLEFT 0.409f //Left Off Track Threshold was .4091
+#define STEERINGREF -0.1f   //Centerpoint reference for steering was -0.1
+
+
+
 //Pins
-DigitalOut myRled(LED1);
-DigitalOut myBled(LED2);
-DigitalOut myBled(LED2);
+    //_p - pin
+    //AI - Analog in
+    //PWM - PWM out
+    //DO - Digital out
+    //IN - Interupt in
+//General Pins
+PwmOut      led_red(LED1);
+PwmOut      led_blue(LED3);
+PwmOut      led_green(LED2);
 
-AnalogIn _SpeedReference(PTB0);
-AnalogIn _Tacho(PTB2);
-PwmOut _MotorControl(PTB1);
-DigitalOut _BreakSignal(PTB3);
+//Motor Pins
+AnalogIn    AI_pSpeedReference(A0);
+AnalogIn    AI_pTacho(A1);
+PwmOut      PWM_pMotorControl(PTD0);
+DigitalOut  DO_pBreakSignal(PTC9);
+AnalogIn    AI_pBattMon(A4);
+
+//Steering Pins
+AnalogIn    AI_pLeftSensors(A2);
+AnalogIn    AI_pRightSensors(A3);
+PwmOut      PWM_pServo(PTE20);
 
-AnalogIn _LeftSensors(A0);
-AnalogIn _RightSensors(A1);
-//AnalogIn _ReferenceSignal(A2);
-PwmOut _servo(PTE20);
+//Binary LED
+DigitalOut  DO_pFirstBinaryLED(A5);
+DigitalOut  DO_pSecondBinaryLED(PTE21);
+DigitalOut  DO_pThirdBinaryLED(PTD2);
+DigitalOut  DO_pFourthBinaryLED(PTE31);
 
+//Hardware Interupts
 Ticker Update;
+InterruptIn IN_pRunButton(PTA5);
+InterruptIn IN_pStopButton(PTA4);
+InterruptIn IN_pBumperStop(PTD3);
+
+InterruptIn IN_pArrowheadLeft(PTD5);
+InterruptIn IN_pArrowheadRight(PTA13);
 
-//Integral Value 
-float _errorArea;
-float _error;
-float _controllerOutput;
-float _ref;
-float _feedbackPrev;
+
+//Data Value
+    //_f - Float
+    //_n - Integer
+    //_b - Boolean
+float   _fErrorArea;
+float   _fError;
+float   _fControllerOutput;
+float   _fMotorReference;
+float   _fFeedbackPrev;
+int     _nCounter;
+bool    _bArrowHitRight;
+bool    _bArrowHitLeft;
+
 
 //States
+    //_o - object
 enum States {RUN, WAIT, STOP, EMPTY};
-States _State;
+States  _oNextState;
+States  _oCurrentState;
 
-//Methods
+//General Methods
 void Init();
 void Loop();
 void UpdateMethod();
+void UpdateLowBatteryStop();
+void OnboardLEDColor(float, float, float);
+void RunButtonMethod();
+void StopButtonMethod();
+void BumperStopMethod();
+
+//Motor Methods
 void UpdateDriveControl();
 void UpdateSteeringControl();
 void SetMotor(float);
 void BreaksOn();
 void BreaksOff();
 
+//Steering Methods
+void UpdateLeft();
+void UpdateRight();
+void UpdateBinaryCounter();
+
+
 //State Methods
-void WaitState();
-void RunState();
-void StopState();
+void TriggerSwitch();
+void StopMethod();
+void WaitMethod();
+void RunMethod();
 
 int main() {
+    cout << "\rCarStart" << endl;
     Init();
-    while(1) {Loop();}
+    while(1) {wait(LOOPTIME); Loop();}
+    //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);}           
 }
 
 void Init() //Initializer
 {
-    _MotorControl.period(FPWM);
-    SetMotor(0.0f); //Turns off the motor
-    _error = 0.0f;
-    _errorArea = 0.0f;
-    _ref = 0.0f;
-    _State = STOP;
-    _feedbackPrev = 0.0f;
-    _servo.period(0.02);
-    BreaksOff();
+    //Variable Init
+    OnboardLEDColor(1.0f,1.0f,1.0f);
+    _fError = ZERO;
+    _fErrorArea = ZERO;
+    _fMotorReference = ZERO;
+    _fFeedbackPrev = ZERO;
+    _nCounter = ZERO;
+    _oNextState = EMPTY;
+    _oCurrentState = EMPTY;
+    _bArrowHitRight = false;
+    _bArrowHitLeft = false;
+    
+    //Motor Init
+    PWM_pMotorControl.period(MOTORPWM);
+    SetMotor(MOTORMIN); //Turns off the motor
+    StopMethod();
+    TriggerSwitch();
+    BreaksOn();
+    
+    //Steering Init
+    PWM_pServo.period(SERVOPWM);
+    
+    //Interupt Init
+    IN_pRunButton.rise(&TriggerSwitch);
+    IN_pStopButton.rise(&StopMethod);
+    IN_pBumperStop.rise(&BumperStopMethod);
+    IN_pArrowheadLeft.rise(&UpdateLeft);
+    IN_pArrowheadRight.rise(&UpdateRight);
     Update.attach(&UpdateMethod, TI);
-    //SetMotor(_ref);
 }
 
 void Loop() //Repeated Loop
 {
-    wait(5);
-    cout<< _ref << endl;
+    _bArrowHitRight = false;
+    _bArrowHitLeft = false;
+    UpdateLowBatteryStop();
 }
 
 void UpdateMethod() //
@@ -90,67 +179,296 @@
     
 void UpdateDriveControl()
 {
-    if(_State == RUN)
+    if(_oCurrentState == RUN)
     {
-    _ref = _SpeedReference.read();
-    _error = _ref - _Tacho.read();
-    _errorArea += TI*_error;
-    _controllerOutput = dKP*_error+ dKI*_errorArea;
-    SetMotor(_controllerOutput);
+    _fMotorReference = AI_pSpeedReference.read();
+    _fError = _fMotorReference - AI_pTacho.read();
+    _fErrorArea += TI*_fError;
+    _fControllerOutput = dKP*_fError+ dKI*_fErrorArea;
     }
+    SetMotor(_fControllerOutput);
 }
 
 void UpdateSteeringControl(void)
 {
+    if(_oCurrentState != STOP){
     //float feedbackPrev = 0;
-    float feedback = _LeftSensors.read() - _RightSensors.read();
-    float reference = 0.0f;//_ReferenceSignal.read();
+    float feedback = AI_pLeftSensors.read() - AI_pRightSensors.read();
+    float reference = STEERINGREF;
     float err = reference - feedback;
-    float feedbackChange = (0.0f/*feedback*/ - _feedbackPrev)/TI;
+    float feedbackChange = (/*0.0f*/feedback - _fFeedbackPrev)/TI;
     float controllerOut = sKP*err + sKD*feedbackChange;
     float controllerOutNorm = (controllerOut + M_PI/2.0f)/M_PI;
     float dutyCycle = DCSLOPE*controllerOutNorm + DCINTERCEPT;
-    if (abs(dutyCycle-_servo.read()) > TOL)
-        _servo.write(dutyCycle);
-    _feedbackPrev = feedback;
+    if (abs(dutyCycle-PWM_pServo.read()) > TOL)
+        PWM_pServo.write(dutyCycle);
+    _fFeedbackPrev = feedback;
+    if (AI_pLeftSensors.read() < OFFTRACKLEFT && AI_pRightSensors.read() < OFFTRACKRIGHT){
+        StopMethod();
+    }
+    }
     
 }
+
+void UpdateLowBatteryStop()
+{
+    float fVal = ZERO;       // variable for the A/D value
+    float fPinVoltage = ZERO; // variable to hold the calculated voltage
+    float fBatteryVoltage = ZERO; //variable for battery voltage
+    fVal = AI_pBattMon.read();    // read the voltage on the divider  
+  
+    fPinVoltage = fVal * ADCRATIO;       //  Calculate the voltage on the A/D pin                          
+                                    
+    fBatteryVoltage = fPinVoltage * BATTRATIO;    //  Use the ratio calculated for the voltage divider
+                                          //  to calculate the battery voltage
+    if(fBatteryVoltage < BATTCUTOFF){
+      cout << "\n\rBattery Voltage is too low. Stop Method " << endl;
+      StopMethod();
+    } 
+    else{
+        cout << "\n\rBattery Voltage is: " << fBatteryVoltage << endl;
+    }
+    
+}
+
 void SetMotor(float speed)
 {
-    float invD = 0.0f;
-    if(speed<0.0f)
-    invD =0.0f;
-    else if(speed> .75f)
-    invD = .75f;
+    float invD = ZERO;
+    
+    if(speed < MOTORMIN) //Speed is above floor
+    invD = MOTORMIN;
+    else if(speed> MOTORMAX) //Caps the speed at a max value
+    invD = MOTORMAX;
     else
     invD = speed;   
  
-    _MotorControl.write(/*1.0f-*/invD);
+    PWM_pMotorControl.write(invD);
 }
 
 void BreaksOn()
 {
-    _controllerOutput = 0;
-    _errorArea = 0;
-    _BreakSignal = 0;
+    SetMotor(MOTORMIN); //Sets speed to Zero
+    _fErrorArea = ZERO; //Resets Error area to zero
+    DO_pBreakSignal = 1; //Turns on breaks
+    PWM_pServo= FULLLEFT; //Turns servo to left
 }
 
 void BreaksOff()
 {
-    _controllerOutput = 0;
-    _errorArea = 0;
-    _BreakSignal = 1;
+    SetMotor(MOTORMIN); //Sets speed to Zero
+    _fErrorArea = ZERO; //Resets Error area to zero
+    DO_pBreakSignal = ZERO; //Turns off brakes
+}
+
+void WaitMethod()
+{
+    _oCurrentState = WAIT;
+    BreaksOn();
+    OnboardLEDColor(.125f, 1.0f, 0.125f); //Sets to Yellow
+    _oNextState = RUN;
+}
+
+void RunMethod()
+{
+    _oCurrentState = RUN;
+    BreaksOff();
+    OnboardLEDColor(1.0f, 1.0f, 0.25f); //Sets to Green
+    _oNextState = WAIT;
+}
+
+void StopMethod()
+{   
+    _oCurrentState = STOP;
+    BreaksOn();
+    OnboardLEDColor(0.25f, 1.0f, 1.0f); //Sets to Red
+    _oNextState = WAIT;
+}
+
+void BumperStopMethod()
+{   
+    _oCurrentState = STOP;
+    BreaksOn();
+    OnboardLEDColor(0.25f, 1.0f, 1.0f); //Sets to Red
+    _oNextState = WAIT;
+}
+
+void LowBatteryStopMethod()
+{
+    _oCurrentState = STOP;
+    BreaksOn();
+    OnboardLEDColor(0.25f, 1.0f, 1.0f); //Sets to Red
+    _oNextState = WAIT;
+}
+
+void TriggerSwitch()
+{
+    switch(_oNextState){
+    case STOP:
+    StopMethod();
+    break;
+    
+    case  WAIT:
+    WaitMethod();
+    break;
+    
+    case RUN:
+    RunMethod();
+    break;
+    
+    case EMPTY:
+    OnboardLEDColor(0.25f, 0.25f, 0.25f); //Sets to White
+    _oNextState = WAIT;
+    break;
+    }
+}
+     
+void UpdateLeft(void)
+{
+ 
+    if(_nCounter >= NLANDMARKS) {
+        _nCounter = 0; //Resets Counter when it reaches the max
+    } else {
+        _nCounter += 1;
+    }
+    if(_bArrowHitRight)
+    {_nCounter = 0;_bArrowHitRight=!_bArrowHitRight;} //Resets Counter on double hit
+    
+    cout << "\n\rcounter =  " << _nCounter << endl;
+    cout << "\n\rLEFT" << endl;
+    
+    UpdateBinaryCounter();
+    _bArrowHitLeft = true;
+}
+    
+void UpdateRight(void)
+{
+ 
+    if(_nCounter >= NLANDMARKS) {
+        _nCounter = 0; //Resets Counter when it reaches the max
+    } else {
+        _nCounter += 1;
+    }
+    if(_bArrowHitLeft)
+    {_nCounter = 0;_bArrowHitLeft=!_bArrowHitLeft;} //Resets Counter on double hit
+    
+    cout << "\n\rcounter =  " << _nCounter << endl;
+    cout << "\n\rRIGHT" << endl;
+    
+    UpdateBinaryCounter();
+    _bArrowHitRight = true;
 }
 
-void WaitState()
-{
-    _State = WAIT;
-}
-void RunState()
+void UpdateBinaryCounter()
 {
-    _State = RUN;
+    switch(_nCounter){
+        case 0:
+            DO_pFirstBinaryLED = LEDOFF;
+            DO_pSecondBinaryLED = LEDOFF;
+            DO_pThirdBinaryLED = LEDOFF;
+            DO_pFourthBinaryLED = LEDOFF;
+        break;
+        case 1:
+            DO_pFirstBinaryLED = LEDON;
+            DO_pSecondBinaryLED = LEDOFF;
+            DO_pThirdBinaryLED = LEDOFF;
+            DO_pFourthBinaryLED = LEDOFF;
+        break; 
+        case 2:
+            DO_pFirstBinaryLED = LEDOFF;
+            DO_pSecondBinaryLED = LEDON;
+            DO_pThirdBinaryLED = LEDOFF;
+            DO_pFourthBinaryLED = LEDOFF;
+        break;
+        case 3:
+            DO_pFirstBinaryLED = LEDON;
+            DO_pSecondBinaryLED = LEDON;
+            DO_pThirdBinaryLED = LEDOFF;
+            DO_pFourthBinaryLED = LEDOFF;
+        break;    
+        case 4:
+            DO_pFirstBinaryLED = LEDOFF;
+            DO_pSecondBinaryLED = LEDOFF;
+            DO_pThirdBinaryLED = LEDON;
+            DO_pFourthBinaryLED = LEDOFF;
+        break;
+        case 5:
+            DO_pFirstBinaryLED = LEDON;
+            DO_pSecondBinaryLED = LEDOFF;
+            DO_pThirdBinaryLED = LEDON;
+            DO_pFourthBinaryLED = LEDOFF;
+        break;    
+        case 6:
+            DO_pFirstBinaryLED = LEDOFF;
+            DO_pSecondBinaryLED = LEDON;
+            DO_pThirdBinaryLED = LEDON;
+            DO_pFourthBinaryLED = LEDOFF;
+        break;
+        case 7:
+            DO_pFirstBinaryLED = LEDON;
+            DO_pSecondBinaryLED = LEDON;
+            DO_pThirdBinaryLED = LEDON;
+            DO_pFourthBinaryLED = LEDOFF;
+        break;
+        case 8:
+            DO_pFirstBinaryLED = LEDOFF;
+            DO_pSecondBinaryLED = LEDOFF;
+            DO_pThirdBinaryLED = LEDOFF;
+            DO_pFourthBinaryLED = LEDON;
+        break;
+        case 9:
+            DO_pFirstBinaryLED = LEDON;
+            DO_pSecondBinaryLED = LEDOFF;
+            DO_pThirdBinaryLED = LEDOFF;
+            DO_pFourthBinaryLED = LEDON;
+        break;
+        case 10:
+            DO_pFirstBinaryLED = LEDOFF;
+            DO_pSecondBinaryLED = LEDON;
+            DO_pThirdBinaryLED = LEDOFF;
+            DO_pFourthBinaryLED = LEDON;
+        break;
+        case 11:
+            DO_pFirstBinaryLED = LEDON;
+            DO_pSecondBinaryLED = LEDON;
+            DO_pThirdBinaryLED = LEDOFF;
+            DO_pFourthBinaryLED = LEDON;
+        break;
+        case 12:
+            DO_pFirstBinaryLED = LEDOFF;
+            DO_pSecondBinaryLED = LEDOFF;
+            DO_pThirdBinaryLED = LEDON;
+            DO_pFourthBinaryLED = LEDON;
+        break;
+        case 13:
+            DO_pFirstBinaryLED = LEDON;
+            DO_pSecondBinaryLED = LEDOFF;
+            DO_pThirdBinaryLED = LEDON;
+            DO_pFourthBinaryLED = LEDON;
+        break;
+        case 14:
+            DO_pFirstBinaryLED = LEDOFF;
+            DO_pSecondBinaryLED = LEDON;
+            DO_pThirdBinaryLED = LEDON;
+            DO_pFourthBinaryLED = LEDON;
+        break;
+        case 15:
+            DO_pFirstBinaryLED = LEDON;
+            DO_pSecondBinaryLED = LEDON;
+            DO_pThirdBinaryLED = LEDON;
+            DO_pFourthBinaryLED = LEDON;
+        break;
+        default:
+            DO_pFirstBinaryLED = LEDOFF;
+            DO_pSecondBinaryLED = LEDOFF;
+            DO_pThirdBinaryLED = LEDOFF;
+            DO_pFourthBinaryLED = LEDOFF;
+        break;               
+    } 
 }
-void StopState()
+
+void OnboardLEDColor(float fRed= 1.0f, float fGreen = 1.0f, float fBlue = 1.0f)
 {
-    _State = STOP;
+    led_red = fRed;
+    led_blue = fGreen;
+    led_green = fBlue;
 }
\ No newline at end of file