Diff: main.cpp

Revision:

0:46582bba07d2

Child:

1:83fccd7d8483

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Oct 17 13:53:44 2016 +0000
@@ -0,0 +1,79 @@
+#include "mbed.h"
+#include "QEI.h"
+
+PwmOut Motor1_pwm(D5);
+QEI Encoder(D12,D13,NC,64);
+Serial pc(USBTX,USBRX);
+Ticker Controller;
+
+float Radius = 10;
+float Frequency = 30;
+float Multiplier = 1;
+float k1 = 1;
+float k2 = 0.1f;
+float k3 = 0.1f;
+float Start_slow = 100;
+float Start_lock = 60;
+float End_slow = 400;
+float End_lock = 440;
+float Frequency_PWM = 10000;
+float Counts = 0;
+float Revolutions = 0;
+const double pi = 3.14159265359;
+
+float Input_force = 0.1;
+float Angle = 0;
+float Curr_speed = 0;
+float Desired_speed;
+float Delta_speed;
+float Int_delta_speed;
+float Deriv_delta_speed = 0;
+float Prev_delta_speed = 0;
+
+void Slide_Controller();
+
+int main()
+{
+    Motor1_pwm.period(1.0/Frequency_PWM);//T=1/f
+   
+    Controller.attach(&Slide_Controller,1/Frequency);
+    pc.baud(9600);
+    while (true) {
+        Counts = Encoder.getPulses();
+        Revolutions = Counts /(32*131);
+        Angle = Revolutions*2*pi;
+        char a = pc.getc();
+        Input_force = (float)a/100;
+            
+        pc.printf("%f", Input_force);
+        //Motor1_pwm.write(Slide_Controller(Input_force));
+        
+        wait(1/Frequency);
+    }
+    return 0;
+}
+
+
+
+
+float Position(float Angle){
+    return Angle*Radius;
+}
+
+void Slide_Controller(){ // Dit ding moet keihard geloopt worden op minstens 30 Hz (Frequency)
+    
+    Desired_speed = Input_force*Multiplier;
+    
+    if (Position(Angle) < Start_slow && Desired_speed < 0){
+        Desired_speed *= (Position(Angle)-Start_lock)/(Start_slow-Start_lock)*1.5-0.5;
+    }
+    if (Position(Angle) > End_slow && Desired_speed > 0){
+        Desired_speed *= (End_lock-Position(Angle))/(End_lock-End_slow)*1.5-0.5;
+    }
+    Prev_delta_speed = Delta_speed;
+    Delta_speed = Desired_speed-Curr_speed;                         // P
+    Int_delta_speed += Delta_speed/Frequency;                       // I
+    Deriv_delta_speed = (Delta_speed-Prev_delta_speed)*Frequency;   // D
+    Motor1_pwm.write(k1*Delta_speed + k2*Int_delta_speed + k3*Deriv_delta_speed);
+    //return k1*Delta_speed + k2*Int_delta_speed + k3*Deriv_delta_speed;
+}
\ No newline at end of file