Diff: main.cpp

Revision:

2:a17f7da1ca7c

Parent:

1:b86f099a314a

Child:

3:b17ae88bfa54

--- a/main.cpp	Tue Jan 22 20:14:53 2019 +0000
+++ b/main.cpp	Tue Mar 26 16:01:53 2019 +0000
@@ -1,22 +1,79 @@
 #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
+
+//Pins
 DigitalOut myRled(LED1);
 DigitalOut myBled(LED2);
-AnalogIn PotCheck(PTE20);
+AnalogIn _Reference(PTB0);
+AnalogIn _Tacho(PTB2);
+PwmOut _MotorControl(PTB1);
+
+Ticker Update;
+
+//Integral Value 
+float _errorArea;
+float _error;
+float _controllerOutput;
+float _ref;
+
+//Methods
+void Init();
+void Loop();
+void UpdateMethod();
+void UpdateControllerOutput();
+void SetMotor(float);
 
 int main() {
-    float fPotValue = 0;
-    while(1) {
-        //myRled = 1;
-        //wait(0.2);
-        myRled = 2*fPotValue;
-        wait(0.2);
-       // myBled = 1;
-        //wait(0.2);
-        myBled = 2*fPotValue;
-        wait(0.2);
-        cout<< "\rI can't do that Carter" << endl;
-        fPotValue = PotCheck.read();
-        cout << fPotValue << endl;
-    }
+    Init();
+    while(1) {Loop();}
+}
+
+void Init() //Initializer
+{
+    _MotorControl.period(FPWM);
+    SetMotor(0.0f); //Turns off the motor
+    _error = 0.0f;
+    _errorArea = 0.0f;
+    _ref = 0.0f;
+    Update.attach(&UpdateMethod, TI);
+    //SetMotor(_ref);
+}
+
+void Loop() //Repeated Loop
+{
+    
+    wait(5);
+    cout<< _ref << endl;
+    
 }
+
+void UpdateMethod() //
+{
+    UpdateControllerOutput();
+} 
+    
+void UpdateControllerOutput()
+{
+    _ref = _Reference.read();
+    _error = _ref - _Tacho.read();
+    _errorArea += TI*_error;
+    _controllerOutput = KP*_error+ KI*_errorArea;
+    SetMotor(_controllerOutput);
+}
+
+void SetMotor(float speed)
+{
+    float invD = 0.0f;
+    if(speed<0.0f)
+    invD =0.0f;
+    else if(speed> .75f)
+    invD = .75f;
+    else
+    invD = speed;   
+ 
+    _MotorControl.write(/*1.0f-*/invD);
+}
\ No newline at end of file