Diff: main.cpp

Revision:

3:11a7da46e093

Parent:

2:dd46c8f3724a

Child:

4:9fdad59c03d6

--- a/main.cpp	Mon Oct 05 16:33:15 2015 +0000
+++ b/main.cpp	Mon Oct 05 18:59:14 2015 +0000
@@ -5,7 +5,8 @@
 //#include "biquadFilter.h"
 #include "encoder.h"
 
-  const double Pi=3.14;
+    /* MODSERIAL to get non-blocking Serial*/
+    MODSERIAL pc(USBTX,USBRX);
 
 void keep_in_range(double * in, double min, double max);
 
@@ -13,7 +14,7 @@
 
 void setlooptimerflag(void);
 
-double get_radians_from_counts(int counts);
+double get_degrees_from_counts(int counts);
 
 double get_setpoint(double input);
 
@@ -22,8 +23,6 @@
     /*Potmeter input*/
     AnalogIn potmeter(A0);
     QEI motor1(D12,D13,NC,32);
-    /* MODSERIAL to get non-blocking Serial*/
-    MODSERIAL pc(USBTX,USBRX);
     /* PWM control to motor */
     PwmOut pwm_motor(D5);
     /* Direction pin */
@@ -58,29 +57,43 @@
 
         looptimerflag = false;
         
-        setpoint = get_setpoint(potmeter.read());
-        
-        pc.printf("setpoint: %d, position motor %d \n\r", setpoint, motor1.getPulses());
+        // Setpoint calibration
+        //setpoint = (potmeter.read()-0.5)*2000;
+        setpoint = 15;
         
-        position=motor1.getPulses();
+        // Position calibration
+        if ((motor1.getPulses()>4200) || (motor1.getPulses()<-4200)) // If value is outside -4200 and 4200 (number of counts equal to one revolution) reset to zero
+        {
+            motor1.reset();
+            pc.printf("RESET \n\r");
+        }   
         
-        keep_in_range(&position,-4200,4200);
+        position= get_degrees_from_counts(motor1.getPulses());
+        
         
-        pc.printf("pwm before keep in range: %d \n\r", position);
+                pc.printf("calibrated setpoint: %f, calibrated position motor %i, position %f \n\r", setpoint, motor1.getPulses(), position);
+        
         
         /* This is a P-action! calculate error, multiply with gain, and store in pwm_to_motor */
-        pwm_to_motor = ((setpoint*1000) - position)*.001;
-
-        pc.printf("pwm before keep in range: %d \n\r", pwm_to_motor);
+        // stel setpoint tussen (0 en 360) en position tussen (0 en 360)
+        // max verschil: 360 -> dan pwm_to_motor 1 tot aan een verschil van 15 graden-> bij 15 moet pwm_to_motor ong 0.1 zijn 
+        // dus     0.1=15*gain      gain=0.0067
+        pwm_to_motor = (setpoint - position)*0.0067;    
+        
         
         keep_in_range(&pwm_to_motor, -1,1);    // Pass to motor controller but keep it in range!
-
-        pc.printf("pwm after keep in range: %d \n\r", pwm_to_motor);
+        pc.printf("pwm %f \n\r", pwm_to_motor);
 
         if(pwm_to_motor > 0)
+            {
             motordir=1;
+            pc.printf("if loop pwm_to_motor > 0 \n\r");
+            }
         else
-            motordir=0; 
+            {
+            motordir=0;
+            pc.printf("else loop pwm_to_motor < 0 \n\r");
+            }
         pwm_motor=(abs(pwm_to_motor));
     }
 }
@@ -99,13 +112,13 @@
 }
 
 // Convert Counts -> Rad       ===> NOG NIET GEBRUIKT
-double get_radians_from_counts(int counts)
+double get_degrees_from_counts(int counts)
 {
 const int gear_ratio =131;
-const int ticks_per_magnet_rotation = 32;//X2
-const double radian_per_encoder_tick =
-2*Pi/(gear_ratio*ticks_per_magnet_rotation);
-return counts * radian_per_encoder_tick;
+const int ticks_per_magnet_rotation = 32;//X2 Encoder
+const double degrees_per_encoder_tick =
+360/(gear_ratio*ticks_per_magnet_rotation);
+return counts * degrees_per_encoder_tick;
 }
 
 // Get setpoint -> potmeter