Diff: main.cpp

Revision:

22:22cb158bcea4

Parent:

21:c856b7752d7d

Child:

23:3306e3267fe6

--- a/main.cpp	Thu Oct 30 12:18:56 2014 +0000
+++ b/main.cpp	Thu Oct 30 13:04:25 2014 +0000
@@ -4,8 +4,8 @@
 #include "PwmOut.h"
 
 #define TSAMP 0.005
-#define K_P (0.8)
-#define K_I (0.01  *TSAMP)
+#define K_P (2.0)
+#define K_I (0.1  *TSAMP)
 #define I_LIMIT 1.
 #define pwm1_min 0.06
 #define pwm2_min 0
@@ -92,39 +92,40 @@
     motor1.setPosition(0);
     pwm_motor1.period_us(100);
     float prev_setpoint = 0;
-    float setpoint;
-    speed1_rad = 0.05;
+    float setpoint = 0;
+    speed1_rad = 2;
 
 
     while(1) {
 
         cur_pos_motor1 = motor1.getPosition();
-        pos_motor1_rad = cur_pos_motor1/(4128/(2*PI));
-
+        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 < -100) {
-            pwm1_percentage = -100;
+        if (pwm1_percentage < -1.0) {
+            pwm1_percentage = -1.0;
         } 
-        else if (pwm1_percentage >100){
-                   pwm1_percentage =100;
-                  }
+        if (pwm1_percentage >1.0){
+            pwm1_percentage =1.0;
+        }
 
-        if(pwm1_percentage < 0) {
-                motordir1 = 1;
-            } 
-            else {
-                motordir1 = 0;
+        if(pwm1_percentage > 0) { 
+            motordir1 = 0; }
+        else{
+            motordir1 = 1;
             }
-
-        if(cur_pos_motor1 < 130) {
-            pwm_motor1.write(abs(pwm1_percentage/100));//(abs(((1-pwm1_min)/100)*pwm1_percentage + pwm1_min));
-            prev_setpoint = setpoint;
-        } 
-        else {
-            pwm_motor1.write(0);
-        }
+        pwm_motor1.write(abs(pwm1_percentage))      ;//(abs(((1-pwm1_min)/100)*pwm1_percentage + pwm1_min));
+        prev_setpoint = setpoint;
+        wait(TSAMP);
     }
 }