Diff: main.cpp

Revision:

24:7fbd904d191c

Parent:

23:3306e3267fe6

Child:

25:f45d61cc6dc6

--- a/main.cpp	Thu Oct 30 13:10:42 2014 +0000
+++ b/main.cpp	Thu Oct 30 20:39:52 2014 +0000
@@ -4,8 +4,8 @@
 #include "PwmOut.h"
 
 #define TSAMP 0.005
-#define K_P (2.0)
-#define K_I (0.05  *TSAMP)
+#define K_P (3.5)
+#define K_I (0.01  *TSAMP)
 #define I_LIMIT 1.
 #define PI 3.14159265
 #define l_arm 0.5
@@ -65,45 +65,76 @@
     out_i += error*K_I;
     clamp(&out_i,-I_LIMIT,I_LIMIT);
     return out_p + out_i;
-    }
+}
+
+/*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);
+}
+
+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;
+}*/
 int main()
 {
-    
-    motor1.setPosition(0);
-    pwm_motor1.period_us(100);
-    float prev_setpoint = 0;
-    float setpoint = 0;
-    speed1_rad = 1.5;
-
-
+        pwm_motor1.period_us(100);
+        motor1.setPosition(0);
+        float prev_setpoint = 0;
+        float setpoint = 0;
+        
     while(1) {
 
-        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 > (2.0*PI)) {
-        setpoint = (2.0*PI); 
-        }
-        if(setpoint < -(2.0*PI)){
-        setpoint = -(2.0*PI); 
-        }
-        
-        
         pwm1_percentage = pid(setpoint, pos_motor1_rad);
         if (pwm1_percentage < -1.0) {
             pwm1_percentage = -1.0;
-        } 
-        if (pwm1_percentage >1.0){
-            pwm1_percentage =1.0;
+        }
+        if (pwm1_percentage > 1.0) {
+            pwm1_percentage = 1.0;
         }
 
-        if(pwm1_percentage > 0) { 
-            motordir1 = 0; }
-        else{
+        if(pwm1_percentage > 0) {
+            motordir1 = 0;
+        } else {
             motordir1 = 1;
-            }
-        pwm_motor1.write(abs(pwm1_percentage))      ;//(abs(((1-pwm1_min)/100)*pwm1_percentage + pwm1_min));
+        }
+        
+        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(setpoint < -(12*2.0*PI/360)) {
+            setpoint = -(12*2.0*PI/360);
+        }
+        
+        pwm_motor1.write(abs(pwm1_percentage));
         prev_setpoint = setpoint;
         wait(TSAMP);
     }