Diff: main.cpp

Revision:

25:f45d61cc6dc6

Parent:

24:7fbd904d191c

Child:

26:5981d4b1144e

--- a/main.cpp	Thu Oct 30 20:39:52 2014 +0000
+++ b/main.cpp	Fri Oct 31 11:27:31 2014 +0000
@@ -4,8 +4,10 @@
 #include "PwmOut.h"
 
 #define TSAMP 0.005
-#define K_P (3.5)
-#define K_I (0.01  *TSAMP)
+#define K_P1 (3.5) //voor motor1 is het 3.5
+#define K_I1 (0.01  *TSAMP) //voor motor1 is het 0.01
+#define K_P2 (1.0)
+#define K_I2 (0.01 *TSAMP)
 #define I_LIMIT 1.
 #define PI 3.14159265
 #define l_arm 0.5
@@ -30,8 +32,8 @@
 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);
+//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 +44,7 @@
 float speed2_rad;
 float pos_motor1_rad;
 float pos_motor2_rad;
-
+int staat = 0;
 
 
 
@@ -55,61 +57,132 @@
 }
 
 
-float pid(float setpoint, float cur_pos_motor1)
+float pid1(float setpoint1, float measurement1)
 {
-    float error;
-    float           out_p = 0;
-    static float    out_i = 0;
-    error  = (setpoint - cur_pos_motor1);
-    out_p  = error*K_P;
-    out_i += error*K_I;
-    clamp(&out_i,-I_LIMIT,I_LIMIT);
-    return out_p + out_i;
+    float error1;
+    float           out_p1 = 0;
+    static float    out_i1 = 0;
+    error1  = (setpoint1 - measurement1);
+    out_p1  = error1*K_P1;
+    out_i1 += error1*K_I1;
+    clamp(&out_i1,-I_LIMIT,I_LIMIT);
+    return out_p1 + out_i1;
 }
 
-/*void batje_links ()
+float prev_setpoint1 = 0;
+float setpoint1 = 0;
+float prev_setpoint2 = 0;
+float setpoint2 = 0;
+
+void batje_links ()
 {
-    speed1_rad = -1.5; //positief is CCW, negatief CW (boven aanzicht)
-        cur_pos_motor1 = motor1.getPosition();
-        pos_motor1_rad = (float)cur_pos_motor1/(4128.0/(2.0*PI));
-        setpoint = prev_setpoint + TSAMP * speed1_rad;
-        if(setpoint > (11.3*2.0*PI/360)) { //setpoint in graden
-            setpoint = (11.3*2.0*PI/360);
-        }
-        if(setpoint < -(11.3*2.0*PI/360)) {
-            setpoint = -(11.3*2.0*PI/360);
-        }
-        
-        pwm_motor1.write(abs(pwm1_percentage));
-        prev_setpoint = setpoint;
-        wait(TSAMP);
+    speed1_rad = -1.0; //positief is CCW, negatief CW (boven aanzicht)
+    cur_pos_motor1 = motor1.getPosition();
+    pos_motor1_rad = (float)cur_pos_motor1/(3600.0/(2.0*PI));
+    setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
+    if(setpoint1 > (180*2.0*PI/360)) { //setpoint in graden
+        setpoint1 = (180*2.0*PI/360);
+    }
+    if(setpoint1 < -(180*2.0*PI/360)) {
+        setpoint1 = -(180*2.0*PI/360);
+    }
+
+    pwm_motor1.write(abs(pwm1_percentage));
+    prev_setpoint1 = setpoint1;
+    wait(TSAMP);
+
 }
 
 void batje_rechts ()
 {
-    speed1_rad = 1.5; //positief is CCW, negatief CW (boven aanzicht)
-        cur_pos_motor1 = motor1.getPosition();
-        pos_motor1_rad = (float)cur_pos_motor1/(4128.0/(2.0*PI));
-        setpoint = prev_setpoint + TSAMP * speed1_rad;
-        if(setpoint > (11.3*2.0*PI/360)) { //setpoint in graden
-            setpoint = (11.3*2.0*PI/360);
-        }
-        if(setpoint < -(11.3*2.0*PI/360)) {
-            setpoint = -(11.3*2.0*PI/360);
-        }
-        pwm_motor1.write(abs(pwm1_percentage));
-        prev_setpoint = setpoint;
-}*/
+    speed1_rad = 2.0; //positief is CCW, negatief CW (boven aanzicht)
+    cur_pos_motor1 = motor1.getPosition();
+    pos_motor1_rad = (float)cur_pos_motor1/(3200.0/(2.0*PI)); //moet 4128
+    setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
+    if(setpoint1 > (360*2.0*PI/360)) { //setpoint in graden
+        setpoint1 = (360*2.0*PI/360);
+    }
+    if(setpoint1 < -(360*2.0*PI/360)) {
+        setpoint1 = -(360*2.0*PI/360);
+    }
+    pwm_motor1.write(abs(pwm1_percentage));
+    prev_setpoint1 = setpoint1;
+    if(setpoint1 > (360*2.0*PI/360)) {
+        staat = 1;
+    }
+    wait(TSAMP);
+
+
+}
+
+void arm_hoog () //LET OP, PAS VARIABELE NOG AAN DIT IS VOOR TESTEN
+{
+    speed1_rad = 6.0; //positief is CCW, negatief CW (boven aanzicht)
+    cur_pos_motor1 = motor1.getPosition();
+    pos_motor1_rad = (float)cur_pos_motor1/(3200.0/(2.0*PI)); //moet 4128
+    setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
+    if(setpoint1 > (155.0*2.0*PI/360)) { //setpoint in graden
+        setpoint1 = (155.0*2.0*PI/360);
+    }
+    if(setpoint1 < -(155.0*2.0*PI/360)) {
+        setpoint1 = -(155.0*2.0*PI/360);
+    }
+    pwm_motor1.write(abs(pwm1_percentage));
+    prev_setpoint1 = setpoint1;
+   //if(setpoint1 = (155.0*2.0*PI/360)) {
+     // staat = 1;
+    //}
+    wait(TSAMP);
+
+}
+
+void arm_mid ()
+{
+    speed1_rad = -3.0; //positief is CCW, negatief CW (boven aanzicht)
+    cur_pos_motor1 = motor1.getPosition();
+    pos_motor1_rad = (float)cur_pos_motor1/(3200.0/(2.0*PI)); //moet 4128
+    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);
+    }
+    pwm_motor1.write(abs(pwm1_percentage));
+    prev_setpoint1 = setpoint1;
+    
+    wait(TSAMP);
+}
+
+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);
+    }
+    if(setpoint2 < -(100*2.0*PI/360)) {
+        setpoint2 = -(100*2.0*PI/360);
+    }
+
+    pwm_motor2.write(abs(pwm2_percentage));
+    prev_setpoint2 = setpoint2;
+    wait(TSAMP);
+}
+
 int main()
 {
-        pwm_motor1.period_us(100);
-        motor1.setPosition(0);
-        float prev_setpoint = 0;
-        float setpoint = 0;
-        
+    pwm_motor1.period_us(100);
+    motor1.setPosition(0);
+    pwm_motor2.period_us(100);
+    motor2.setPosition(0);
+    pc.baud(115200);
+
     while(1) {
-
-        pwm1_percentage = pid(setpoint, pos_motor1_rad);
+        pc.printf("%d \r\n", motor1.getPosition());
+        pwm1_percentage = pid1(setpoint1, pos_motor1_rad);
         if (pwm1_percentage < -1.0) {
             pwm1_percentage = -1.0;
         }
@@ -122,23 +195,16 @@
         } else {
             motordir1 = 1;
         }
-        
-        speed1_rad = 1.0; //positief is CCW, negatief CW (boven aanzicht)
-        cur_pos_motor1 = motor1.getPosition();
-        pos_motor1_rad = (float)cur_pos_motor1/(4128.0/(2.0*PI));
-        setpoint = prev_setpoint + TSAMP * speed1_rad;
-        if(setpoint > (12*2.0*PI/360)) { //setpoint in graden
-            setpoint = (12*2.0*PI/360);
+
+
+        if(staat == 0) {
+            arm_hoog();
+        } 
+        else 
+        {
+            arm_mid();
         }
-        if(setpoint < -(12*2.0*PI/360)) {
-            setpoint = -(12*2.0*PI/360);
-        }
-        
-        pwm_motor1.write(abs(pwm1_percentage));
-        prev_setpoint = setpoint;
-        wait(TSAMP);
-    }
+       }
 }
 
 
-