Diff: main.cpp

Revision:

27:77bb255753be

Parent:

26:5981d4b1144e

--- a/main.cpp	Fri Oct 31 12:06:01 2014 +0000
+++ b/main.cpp	Fri Oct 31 13:04:51 2014 +0000
@@ -6,8 +6,8 @@
 #define TSAMP 0.005
 #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 K_P2 (1.0)  //3.5
+#define K_I2 (0.01 *TSAMP) //0.1
 #define I_LIMIT 1.
 #define PI 3.14159265
 #define l_arm 0.5
@@ -44,7 +44,7 @@
 float speed2_rad;
 float pos_motor1_rad;
 float pos_motor2_rad;
-int staat = 0;
+int staat1 = 0;
 
 
 
@@ -69,6 +69,18 @@
     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_P1;
+    out_i2 += error2*K_I1;
+    clamp(&out_i2,-I_LIMIT,I_LIMIT);
+    return out_p2 + out_i2;
+}
+
 float prev_setpoint1 = 0;
 float setpoint1 = 0;
 float prev_setpoint2 = 0;
@@ -76,49 +88,34 @@
 
 void batje_links ()
 {
-    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));
+    
+    speed1_rad = 6.0; //positief is CCW, negatief CW (boven aanzicht)
     setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
-    if(setpoint1 > (180*2.0*PI/360)) { //setpoint in graden
-        setpoint1 = (180*2.0*PI/360);
+    if(setpoint1 > (155.0*2.0*PI/360)) { //setpoint in graden
+        setpoint1 = (155.0*2.0*PI/360);
     }
-    if(setpoint1 < -(180*2.0*PI/360)) {
-        setpoint1 = -(180*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;
-    wait(TSAMP);
-
 }
 
 void batje_rechts ()
 {
-    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
+    speed1_rad = 6.0; //positief is CCW, negatief CW (boven aanzicht)
     setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
-    if(setpoint1 > (360*2.0*PI/360)) { //setpoint in graden
-        setpoint1 = (360*2.0*PI/360);
+    if(setpoint1 > (155.0*2.0*PI/360)) { //setpoint in graden
+        setpoint1 = (155.0*2.0*PI/360);
     }
-    if(setpoint1 < -(360*2.0*PI/360)) {
-        setpoint1 = -(360*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 > (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)
- 
     setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
     if(setpoint1 > (155.0*2.0*PI/360)) { //setpoint in graden
         setpoint1 = (155.0*2.0*PI/360);
@@ -126,20 +123,15 @@
     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)-0.1) {
-     staat = 1;
+     staat1 = 1;
     }
-    
-
 }
 
 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
     setpoint1 = prev_setpoint1 + (TSAMP * speed1_rad);
     if(setpoint1 > (155*2.0*PI/360)) { //setpoint in graden
         setpoint1 = (155*2.0*PI/360);
@@ -147,31 +139,36 @@
     if(setpoint1 < -(155*2.0*PI/360)) {
         setpoint1 = -(155*2.0*PI/360);
     }
-    //pc.printf("m%f\n\r",setpoint1);
-    //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);
+    
+    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);
     }
-    if(setpoint2 < -(100*2.0*PI/360)) {
-        setpoint2 = -(100*2.0*PI/360);
+    if(setpoint1 < -(155.0*2.0*PI/360)) {
+        setpoint1 = -(155.0*2.0*PI/360);
     }
-
-    pwm_motor2.write(abs(pwm2_percentage));
-    prev_setpoint2 = setpoint2;
-    wait(TSAMP);
+    prev_setpoint1 = setpoint1;   
 }
 
+void arm_begin ()
+{
+    
+    speed1_rad = 1.0; //positief is CCW, negatief CW (boven aanzicht)
+    setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
+    if(setpoint1 > (0.0*2.0*PI/360)) { //setpoint in graden
+        setpoint1 = (0.0*2.0*PI/360);
+    }
+    if(setpoint1 < -(0.0*2.0*PI/360)) {
+        setpoint1 = -(0.0*2.0*PI/360);
+    }
+    prev_setpoint1 = setpoint1;
+}
 int main()
 {
     int wait_iterator = 0;
@@ -182,38 +179,64 @@
     pc.baud(115200);
 
     while(1) {
+        
+        pc.printf("%d, %f \r\n", motor1.getPosition(), motor2.getPosition());
         //MOTOR1
-        pc.printf("%d \r\n", motor1.getPosition());
         cur_pos_motor1 = motor1.getPosition();
-        pos_motor1_rad = (float)cur_pos_motor1/(3200.0/(2.0*PI)); //moet 4128
+        pos_motor1_rad = (float)cur_pos_motor1/(4128.0/(2.0*PI)); //moet 4128
         pwm1_percentage = pid1(setpoint1, pos_motor1_rad);
+        
+        
         if (pwm1_percentage < -1.0) {
             pwm1_percentage = -1.0;
         }
         if (pwm1_percentage > 1.0) {
             pwm1_percentage = 1.0;
         }
+        
         pwm_motor1.write(abs(pwm1_percentage));
         if(pwm1_percentage > 0) {
             motordir1 = 0;
         } else {
             motordir1 = 1;
         }
+        
+        //MOTOR 2
+        cur_pos_motor2 = motor2.getPosition();
+        pos_motor2_rad = (float)cur_pos_motor2/(3200.0/(2.0*PI)); //moet 4128
+        pwm2_percentage = pid2(setpoint2, pos_motor2_rad);
+        
+        if (pwm2_percentage < -1.0) {
+            pwm2_percentage = -1.0;
+        }
+        if (pwm2_percentage > 1.0) {
+            pwm2_percentage = 1.0;
+        }
+        
+        pwm_motor2.write(abs(pwm2_percentage));
+        if(pwm2_percentage > 0) {
+            motordir2 = 0;
+        } else {
+            motordir2 = 1;
+        }
+        
         wait(TSAMP);
+        
+        
         //STATES
-        if(staat == 0) {
+        if(staat1 == 0) {
             arm_hoog();
             wait_iterator = 0;
         } 
-        else if(staat ==1)
+        else if(staat1 == 1)
         {
             wait_iterator++;
             if(wait_iterator > 200)
-                staat = 2;
+                staat1 = 2;
         }
         else
         {
-            arm_mid();
+            arm_begin();
         }
        }
 }