Revision 2:68de33d10c67, committed 2017-10-11

Comitter:

DiondeGreef

Date:

Wed Oct 11 09:49:09 2017 +0000

Parent:

1:92a60278860a

Commit message:

alkdfjakljasdfladksfj;

Changed in this revision

diff -r 92a60278860a -r 68de33d10c67 Encoder.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Encoder.lib	Wed Oct 11 09:49:09 2017 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/vsluiter/code/Encoder/#18b000b443af

diff -r 92a60278860a -r 68de33d10c67 feed_forward.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/feed_forward.lib	Wed Oct 11 09:49:09 2017 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/DiondeGreef/code/feed_forward/#92a60278860a

diff -r 92a60278860a -r 68de33d10c67 main.cpp
--- a/main.cpp	Tue Oct 03 15:35:26 2017 +0000
+++ b/main.cpp	Wed Oct 11 09:49:09 2017 +0000
@@ -1,89 +1,56 @@
 #include "mbed.h"
 #include "MODSERIAL.h"
 #include "math.h"
+#include "encoder.h"
 
 DigitalOut gpo(D0);
 DigitalOut motorDirection(D4);
 PwmOut motorSpeed(D5);
-AnalogIn potMeterIn(A1);
-InterruptIn button1(D3);
-Ticker ticker;
+AnalogIn potMeterIn1(A1);
+AnalogIn potMeterIn2(A2);
+//InterruptIn button1(D3);
+Ticker m1_Ticker;
+Encoder encoder1(D13,D12);
+const double M1_KP = 2.5, M1_KI = 1.0;
+const double M1_TS = 0.01;
+const double RAD_PER_PULSE = 0.002991;
+double m1_err_int = 0;
+int motorD = 0;
+double motor1 = 0;
 
 MODSERIAL pc(USBTX, USBRX);
 
 
-float GetReferenceVelocity()
-{
-    // Returns reference velocity in rad/s. 
-    // Positive value means clockwise rotation.
-    const float maxVelocity=8.4; // in rad/s of course!  
-    float referenceVelocity;  // in rad/s
-    if (button1)   {
-        // Clockwise rotation  
-        referenceVelocity = potMeterIn * maxVelocity;
-        } 
-    else {
-        // Counterclockwise rotation
-        referenceVelocity = -1*potMeterIn * maxVelocity;
-          }
-    return referenceVelocity;
+                                                                                    // Reusable PI controller
+double PI( double e, const double Kp, const double Ki, double Ts, double &e_int ){
+    e_int += Ts ∗ e;                                                                // e_int is changed globally because it’s ’by reference’ (&)
+    return Kp ∗ e + Ki ∗ e_int;
 }
-
-void setMotor(float motorValue) {
-        if (motorValue >= 0) 
+                                                                                    // Next task, measure the error and apply the output to the plant
+void m1_Controller() {
+    double reference = potMeterIn1;
+    
+    
+    double position = RAD_PER_PULSE∗encoder1;                         // Don’t use magic numbers!
+    motor1 = PI( reference − position, M1_KP, M1_KI, M1_TS, m1_err_int );
+    if ( reference − position >= 0) 
         {
             //float motor1DirectionPin1 = 1;
-            motorDirection=1;
+            motorD=1;
         }
         else
         {
             //float motor1DirectionPin1 = 0;
-            motorDirection=0;
+            motorD=0;
         }
-        
-        if (fabs(motorValue)>1)
-        {
-          //float motor1MagnitudePin1 = 1;
-            motorSpeed = 1;
-         }
-         else  
-         {
-             //float motor1MagnitudePin1 = fabs(motorValue); 
-             motorSpeed = fabs(motorValue); 
-        }        
-}
-
-float FeedForwardControl(float referenceVelocity)
-{
-    // very simple linear feed-forward control
-    const float MotorGain=8.4; // unit: (rad/s) / PWM
-    float motorValue = referenceVelocity / MotorGain;
-    return motorValue;
-}
-
-
-void MeasureAndControl(void)
-{
-    // This function measures the potmeter position, extracts a
-    // reference velocity from it, and controls the motor with 
-    // a simple FeedForward controller. Call this from a Ticker.
-    float referenceVelocity = GetReferenceVelocity();
-    float motorValue = FeedForwardControl(referenceVelocity);
-    setMotor(motorValue);
 }
 
 int main() {
-    pc.baud(115200);
-    //ticker.attach(MeasureAndControl, 0.01);
-    while(true){
-        wait(0.1);
-        
-        //pc.printf("%f\r\n",GetReferenceVelocity());
-        float v_ref = GetReferenceVelocity();
-        setMotor(v_ref);
-        pc.printf("%f \r\n", FeedForwardControl(v_ref));
-        motorDirection.write(motorDirection);
-        motorSpeed.write(motorSpeed);   //PWM Speed Control
+    m1_Ticker.attach( &m1_Controller, M1_TS );                                      // 100 Hz
+    while( 1 ) {
+        motorDirection.write(motorD);
+        motorSpeed.write(motor1);        
     }
 }
+}
     
\ No newline at end of file