Diff: main.cpp

Revision:

3:b17ae88bfa54

Parent:

2:a17f7da1ca7c

Child:

4:11559b04c4ff

--- a/main.cpp	Tue Mar 26 16:01:53 2019 +0000
+++ b/main.cpp	Wed Mar 27 17:24:22 2019 +0000
@@ -1,16 +1,32 @@
 #include "mbed.h"
 #include <iostream>
 #define TI  .001f       //1kHz sample time
-#define KP  5.1491f     //Proportional Gain
-#define KI  119.3089f   //Integral Gain
-#define FPWM .00004f    //PWM Frequency
+#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
+#ifndef M_PI
+#define M_PI 3.14f
+#endif
 
 //Pins
 DigitalOut myRled(LED1);
 DigitalOut myBled(LED2);
-AnalogIn _Reference(PTB0);
+DigitalOut myBled(LED2);
+
+AnalogIn _SpeedReference(PTB0);
 AnalogIn _Tacho(PTB2);
 PwmOut _MotorControl(PTB1);
+DigitalOut _BreakSignal(PTB3);
+
+AnalogIn _LeftSensors(A0);
+AnalogIn _RightSensors(A1);
+//AnalogIn _ReferenceSignal(A2);
+PwmOut _servo(PTE20);
 
 Ticker Update;
 
@@ -19,13 +35,26 @@
 float _error;
 float _controllerOutput;
 float _ref;
+float _feedbackPrev;
+
+//States
+enum States {RUN, WAIT, STOP, EMPTY};
+States _State;
 
 //Methods
 void Init();
 void Loop();
 void UpdateMethod();
-void UpdateControllerOutput();
+void UpdateDriveControl();
+void UpdateSteeringControl();
 void SetMotor(float);
+void BreaksOn();
+void BreaksOff();
+
+//State Methods
+void WaitState();
+void RunState();
+void StopState();
 
 int main() {
     Init();
@@ -39,32 +68,53 @@
     _error = 0.0f;
     _errorArea = 0.0f;
     _ref = 0.0f;
+    _State = STOP;
+    _feedbackPrev = 0.0f;
+    _servo.period(0.02);
+    BreaksOff();
     Update.attach(&UpdateMethod, TI);
     //SetMotor(_ref);
 }
 
 void Loop() //Repeated Loop
 {
-    
     wait(5);
     cout<< _ref << endl;
-    
 }
 
 void UpdateMethod() //
 {
-    UpdateControllerOutput();
+    UpdateDriveControl();
+    UpdateSteeringControl();
 } 
     
-void UpdateControllerOutput()
+void UpdateDriveControl()
 {
-    _ref = _Reference.read();
+    if(_State == RUN)
+    {
+    _ref = _SpeedReference.read();
     _error = _ref - _Tacho.read();
     _errorArea += TI*_error;
-    _controllerOutput = KP*_error+ KI*_errorArea;
+    _controllerOutput = dKP*_error+ dKI*_errorArea;
     SetMotor(_controllerOutput);
+    }
 }
 
+void UpdateSteeringControl(void)
+{
+    //float feedbackPrev = 0;
+    float feedback = _LeftSensors.read() - _RightSensors.read();
+    float reference = 0.0f;//_ReferenceSignal.read();
+    float err = reference - feedback;
+    float feedbackChange = (0.0f/*feedback*/ - _feedbackPrev)/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;
+    
+}
 void SetMotor(float speed)
 {
     float invD = 0.0f;
@@ -76,4 +126,31 @@
     invD = speed;   
  
     _MotorControl.write(/*1.0f-*/invD);
+}
+
+void BreaksOn()
+{
+    _controllerOutput = 0;
+    _errorArea = 0;
+    _BreakSignal = 0;
+}
+
+void BreaksOff()
+{
+    _controllerOutput = 0;
+    _errorArea = 0;
+    _BreakSignal = 1;
+}
+
+void WaitState()
+{
+    _State = WAIT;
+}
+void RunState()
+{
+    _State = RUN;
+}
+void StopState()
+{
+    _State = STOP;
 }
\ No newline at end of file