Diff: main.cpp

Revision:

28:4f2e15118f13

Parent:

26:5981d4b1144e

Child:

29:4b7f6f15cedf

--- a/main.cpp	Fri Oct 31 12:06:01 2014 +0000
+++ b/main.cpp	Fri Oct 31 13:23:51 2014 +0000
@@ -32,8 +32,7 @@
 Encoder motor2(PTD2, PTD0); //wit, geel
 PwmOut pwm_motor2(M1_PWM);
 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;
@@ -46,9 +45,6 @@
 float pos_motor2_rad;
 int staat = 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.
 {
@@ -56,7 +52,6 @@
     // *in = het getal dat staat op locatie van in --> waarde van new_pwm
 }
 
-
 float pid1(float setpoint1, float measurement1)
 {
     float error1;
@@ -69,6 +64,17 @@
     return out_p1 + out_i1;
 }
 
+float pid2(float setpoint2, float measurement2)
+{
+    float error2;
+    float           out_p2 = 0;
+    static float    out_i2 = 0;
+    error2  = (setpoint2 - measurement2);
+    out_p2  = error2*K_P2;
+    out_i2 += error2*K_I2;
+    clamp(&out_i2,-I_LIMIT,I_LIMIT);
+    return out_p2 + out_i2;
+}
 float prev_setpoint1 = 0;
 float setpoint1 = 0;
 float prev_setpoint2 = 0;
@@ -118,7 +124,6 @@
 void arm_hoog () //LET OP, PAS VARIABELE NOG AAN DIT IS VOOR TESTEN
 {
     speed1_rad = 6.0; //positief is CCW, negatief CW (boven aanzicht)
- 
     setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
     if(setpoint1 > (155.0*2.0*PI/360)) { //setpoint in graden
         setpoint1 = (155.0*2.0*PI/360);
@@ -137,18 +142,14 @@
 
 void arm_mid ()
 {
-    speed1_rad = -3.0; //positief is CCW, negatief CW (boven aanzicht)
-    //cur_pos_motor1 = motor1.getPosition();
-    //os_motor1_rad = (float)cur_pos_motor1/(3200.0/(2.0*PI)); //moet 4128
+    speed1_rad = -2.0; //positief is CCW, negatief CW (boven aanzicht)
     setpoint1 = prev_setpoint1 + (TSAMP * speed1_rad);
     if(setpoint1 > (155*2.0*PI/360)) { //setpoint in graden
         setpoint1 = (155*2.0*PI/360);
     }
-    if(setpoint1 < -(155*2.0*PI/360)) {
-        setpoint1 = -(155*2.0*PI/360);
+    if(setpoint1 < -(0.0*2.0*PI/360)) {
+        setpoint1 = -(0.0*2.0*PI/360);
     }
-    //pc.printf("m%f\n\r",setpoint1);
-    //pwm_motor1.write(abs(pwm1_percentage));
     prev_setpoint1 = setpoint1;
     
     //wait(TSAMP);
@@ -156,20 +157,28 @@
 
 void arm_laag ()
 {
-    speed2_rad = -1.0; //positief is CCW, negatief CW (boven aanzicht)
-    cur_pos_motor2 = motor2.getPosition();
-    pos_motor2_rad = (float)cur_pos_motor2/(3600/(2.0*PI));
-    setpoint2 = prev_setpoint2 + TSAMP * speed2_rad;
-    if(setpoint2 > (100*2.0*PI/360)) { //setpoint in graden
-        setpoint2 = (100*2.0*PI/360);
+    speed1_rad = -2.0; //positief is CCW, negatief CW (boven aanzicht)
+    setpoint1 = prev_setpoint1 + (TSAMP * speed1_rad);
+    if(setpoint1 > (155*2.0*PI/360)) { //setpoint in graden
+        setpoint1 = (155*2.0*PI/360);
+    }
+    if(setpoint1 < -(0.0*2.0*PI/360)) {
+        setpoint1 = -(0.0*2.0*PI/360);
     }
-    if(setpoint2 < -(100*2.0*PI/360)) {
-        setpoint2 = -(100*2.0*PI/360);
-    }
+    prev_setpoint1 = setpoint1;
+}
 
-    pwm_motor2.write(abs(pwm2_percentage));
-    prev_setpoint2 = setpoint2;
-    wait(TSAMP);
+void arm_begin ()
+{
+    speed1_rad = -2.0; //positief is CCW, negatief CW (boven aanzicht)
+    setpoint1 = prev_setpoint1 + (TSAMP * speed1_rad);
+    if(setpoint1 > (155*2.0*PI/360)) { //setpoint in graden
+        setpoint1 = (155*2.0*PI/360);
+    }
+    if(setpoint1 < -(0.0*2.0*PI/360)) {
+        setpoint1 = -(0.0*2.0*PI/360);
+    }
+    prev_setpoint1 = setpoint1;
 }
 
 int main()
@@ -213,7 +222,7 @@
         }
         else
         {
-            arm_mid();
+            arm_begin();
         }
        }
 }