Diff: main.cpp

Revision:

0:d566a0281325

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Oct 22 08:33:23 2018 +0000
@@ -0,0 +1,45 @@
+#include "mbed.h"
+#include "HIDScope.h"
+
+DigitalOut gpo(D0);
+DigitalOut led(LED_RED);
+HIDScope scope(2);
+
+const float Kp = 2;
+const float Ki = 0.2;
+const float Kd = 0;
+const float Timestep = 0.001;
+float G = 15;          //input
+float Output = 0 ; //Starting value
+float P = 0 ; // Starting value
+float e1 = 0 ; //Starting value 
+float e2 = 0 ; // Starting value
+float e3;       
+float Output_Last;  // Remember previous position
+float Y;            // Value that is outputted to motor control
+float P_Last = 0;   // Starting position
+const float Max_Speed = 400;      //Max speed of the motor
+int main()
+{
+    while (true) {
+         P_Last = P;        
+         e1 = e2;
+         e2 = e3;
+         e3 = G - P;
+         Output_Last = Output;
+         Output = Kp * (e3 - e2) + Output_Last +Ki * e3 + Kd * (e3 - 2*e2 + e1);
+             Y = Output;
+        if (Output >= 1){
+            Y = 1;
+        }
+        else if (Output <= -1){
+            Y = -1;
+        }
+        P = P_Last + Y * Timestep * Max_Speed;
+    wait(0.1f);
+    scope.set(0,Output);
+    scope.set(1,P);
+    scope.send();
+    }
+    
+}
\ No newline at end of file