Diff: main.cpp

Revision:

19:3e46c457091c

Parent:

18:fb85c58a4106

Child:

20:25e5916fe7b1

Child:

21:c856b7752d7d

--- a/main.cpp	Thu Oct 30 09:42:58 2014 +0000
+++ b/main.cpp	Thu Oct 30 11:45:48 2014 +0000
@@ -1,13 +1,13 @@
 #include "mbed.h"
 #include "encoder.h"
 #include "HIDScope.h"
+#include "PwmOut.h"
 
 #define TSAMP 0.005
-#define K_P (0.1)
-#define K_I (0.03  *TSAMP)
-#define K_D (0.001 /TSAMP)
+#define K_P (0.8)
+#define K_I (0.01  *TSAMP)
 #define I_LIMIT 1.
-#define pwm1_min 0
+#define pwm1_min 0.06
 #define pwm2_min 0
 #define PI 3.14159265
 #define l_arm 0.5
@@ -34,7 +34,6 @@
 DigitalOut motordir2(M1_DIR);
 void clamp(float * in, float min, float max);
 float pid(float setpoint, float measurement);
-
 float pwm1_percentage = 0;
 float pwm2_percentage = 0;
 int cur_pos_motor1;
@@ -42,7 +41,9 @@
 int cur_pos_motor2;
 int prev_pos_motor2 = 0;
 float speed1_rad;
-float speed2_rad; 
+float speed2_rad;
+float pos_motor1_rad;
+float pos_motor2_rad;
 
 
 
@@ -68,18 +69,51 @@
 
 int main()
 {
-    Ticker log_timer;
-    log_timer.attach(looper, TSAMP);
+    //Ticker log_timer;
+    //log_timer.attach(looper, TSAMP);
+    motor1.setPosition(0);
+    pwm_motor1.period_us(100);
+    float prev_setpoint = 0;
+    float setpoint;
+    speed1_rad = 0.29;
+
 
     while(1) {
 
+        cur_pos_motor1 = motor1.getPosition();
+        pos_motor1_rad = cur_pos_motor1/(4128/(2*PI));
+
+        setpoint = prev_setpoint + TSAMP * speed1_rad;
+        pwm1_percentage = pid(setpoint, pos_motor1_rad);
+
+        if (pwm1_percentage < -100) {
+            pwm1_percentage = -100;
+        } 
+        else if (pwm1_percentage >100){
+                   pwm1_percentage =100;
+                  }
+
+        if(pwm1_percentage < 0) {
+                motordir1 = 1;
+            } 
+            else {
+                motordir1 = 0;
+            }
+
+        if(cur_pos_motor1 < 1032) {
+            pwm_motor1.write(pwm1_percentage/100);//(abs(((1-pwm1_min)/100)*pwm1_percentage + pwm1_min));
+            prev_setpoint = setpoint;
+        } 
+        else {
+            pwm_motor1.write(0);
+        }
     }
 }
 
 void clamp(float* in, float min, float max) // "*" is een pointer (verwijst naar het adres waar een variabele instaat). Dus je slaat niet de variabele op
-// maar de locatie van de variabele. 
+// maar de locatie van de variabele.
 {
-    *in > min ? /*(*/*in < max? /*niets doen*/ : *in = max/*)*/: *in = min; // a ? b : c --> als a waar is, dan doe je b, en anders c 
+*in > min ? /*(*/*in < max? /*niets doen*/ : *in = max/*)*/: *in = min; // a ? b : c --> als a waar is, dan doe je b, en anders c
     // *in = het getal dat staat op locatie van in --> waarde van new_pwm
 }
 
@@ -87,13 +121,11 @@
 float pid(float setpoint, float measurement)
 {
     float error;
-    static float prev_error = 0;
     float           out_p = 0;
     static float    out_i = 0;
     error  = (setpoint-measurement);
-    out_p  = error*K_P; 
+    out_p  = error*K_P;
     out_i += error*K_I;
     clamp(&out_i,-I_LIMIT,I_LIMIT);
-    prev_error = error;
-    return out_p + out_i + out_d;
+    return out_p + out_i;
 }