Diff: main.cpp

Revision:

7:ddd7fb357786

Parent:

6:8a62f76a1f68

Child:

8:50d6e2323d3b

--- a/main.cpp	Mon Oct 05 19:14:19 2015 +0000
+++ b/main.cpp	Tue Oct 06 11:59:45 2015 +0000
@@ -5,58 +5,49 @@
 //#include "biquadFilter.h"
 #include "encoder.h"
 
-    /* MODSERIAL to get non-blocking Serial*/
-    MODSERIAL pc(USBTX,USBRX);
-    DigitalIn button(PTA4); // PTA4 is used as a button controller (0 when pressed and 1 when not pressed)
-
-void keep_in_range(double * in, double min, double max);
+MODSERIAL pc(USBTX,USBRX);
+DigitalIn button(PTA4); // PTA4 is used as a button controller (0 when pressed and 1 when not pressed)
 
 volatile bool looptimerflag;
-
-void setlooptimerflag(void);
+const double cw=0;          // zero is clockwise (front view)
+const double ccw=1;         // one is counterclockwise (front view)
 
+// Functions used (described after main)
+void keep_in_range(double * in, double min, double max);
+void setlooptimerflag(void);
 double get_degrees_from_counts(int counts);
-
 double get_setpoint(double input);
 
+// MAIN function
 int main() {
-    //LOCAL VARIABLES 
-    /*Potmeter input*/
     AnalogIn potmeter(A0);
     QEI motor1(D12,D13,NC,32);
-    /* PWM control to motor */
     PwmOut pwm_motor(D5);
-    /* Direction pin */
     DigitalOut motordir(D4);
-    /* variable to store setpoint in */
     double setpoint;
-    /* variable to store pwm value in*/
     double pwm_to_motor;
-    /* variable to store position of the motor in */
     double position;
     
-    //START OF CODE
-    
-    pc.printf("bla \n\r");
+    //START OF CODE 
+    pc.printf("Start of code \n\r");
     
-    /*Set the baudrate (use this number in RealTerm too! */
-    pc.baud(9600);
+    pc.baud(9600);          // Set the baudrate
     
-   /*Create a ticker, and let it call the     */
-   /*function 'setlooptimerflag' every 0.01s  */
-    Ticker looptimer;
-    
+    // Tickers 
+    Ticker looptimer;                             // Ticker called looptimer to set a looptimerflag
     looptimer.attach(setlooptimerflag,0.01);      // calls the looptimer flag every 0.01s
     
-    pc.printf("bla \n\r");
+    pc.printf("Start infinite loop \n\r");
+    wait (3);
     
     //INFINITE LOOP
     
     while(1) 
-        {
-         if (button.read() < 0.5) {   //if button pressed
-            motordir = 0; // zero is clockwise (front view)
-            pwm_motor = 0.5f; // motorspeed
+        {                                                                           // Start while loop
+         if (button.read() < 0.5){                   //if button pressed
+             motordir = cw;                          // zero is clockwise (front view)
+             pwm_motor = 0.5f;                       // motorspeed
+            
             pc.printf("positie = %d \r\n", motor1.getPulses()); }
         else
         {
@@ -75,31 +66,38 @@
             motor1.reset();
             pc.printf("RESET \n\r");
         }   
+             
+// gear ratio motor = 131
+// ticks per magnet rotation = 32 (X2 Encoder)
+// One revolution = 360 degrees
+// degrees_per_encoder_tick = 360/(gear_ratio*ticks_per_magnet_rotation)=360/131*32=0.085877862594198 
+
         double conversion_to_degree_counts=0.085877862594198; 
         position = conversion_to_degree_counts * motor1.getPulses();
         
         
-                pc.printf("calibrated setpoint: %f, calibrated position motor %i, position %f \n\r", setpoint, motor1.getPulses(), 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
         // 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;    
         
+        pwm_to_motor = (setpoint - position)*0.0067 + e_prev;    
+        e_prev=(setpoint - position)
         
         keep_in_range(&pwm_to_motor, -1,1);    // Pass to motor controller but keep it in range!
         pc.printf("pwm %f \n\r", pwm_to_motor);
 
         if(pwm_to_motor > 0)
             {
-            motordir=1;
+            motordir=ccw;
             pc.printf("if loop pwm_to_motor > 0 \n\r");
             }
         else
             {
-            motordir=0;
+            motordir=cw;
             pc.printf("else loop pwm_to_motor < 0 \n\r");
             }
         pwm_motor=(abs(pwm_to_motor));
@@ -107,7 +105,6 @@
 }
 }
 
-
 // Keep in range function
 void keep_in_range(double * in, double min, double max)
 {
@@ -120,16 +117,6 @@
     looptimerflag = true;
 }
 
-// Convert Counts -> Rad       ===> NOG NIET GEBRUIKT
-double get_degrees_from_counts(int counts)
-{
-const int gear_ratio =131;
-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
 double get_setpoint(double input)
 {