Diff: main.cpp

Revision:

21:dd4bbb617415

Parent:

20:c3bdb8f73c02

Child:

24:487c898c8d71

--- a/main.cpp	Tue Mar 14 23:40:16 2017 +0000
+++ b/main.cpp	Wed Mar 15 17:03:31 2017 +0000
@@ -5,15 +5,16 @@
 #include "parser.h"
 #include <cmath>
 
-#define kp 0.3f
-#define ki 0.05f
-#define kd 0.5f
-#define dt 0.02f                        //given in ms, used to call the PID c.
+#define kp 0.08f
+#define ki 0.0f //0.05f
+#define kd 0.0f //0.5f
+#define dt 0.002f                        //given in ms, used to call the PID c.
 
 volatile uint8_t state = 0;
 volatile float w3 = 0;                  //Angular velocity
-volatile float duty = 0.40;
+volatile float duty = 0.5;
 volatile int count_i3 = 0;
+volatile int prev_count_i3 = -1;
 const float angularVelocities[29] = {0, 0, 0, 56.3705020, 153.953598, 
 221.162308, 436.561981, 652.034058, 669.472534, 671.117249, 703.662231, 
 704.767273, 695.868835, 676.609924, 689.303345, 685.318481, 680.420166, 
@@ -30,7 +31,7 @@
 Ticker controlTicker;
 
 volatile float fi0 = 0;                 //number of revs done
-volatile int goalRevs = 200;
+volatile int goalRevs = 400;
 volatile float fi = 2*3.1415*goalRevs;
 volatile float goalW = 0;               //desired angular velocity
 volatile float accError = 0;
@@ -47,7 +48,7 @@
             }
         }
     }
-    return 0;
+    return dutyCycles[28];
 }
 
 void control(){
@@ -69,7 +70,7 @@
     w3 = angle/dt_I3.read();            //Calc angular velocity
     
     dt_I3.reset();
-    count_i3++;                   
+    count_i3++;                 
 }
 
 void i_edge(){
@@ -90,7 +91,7 @@
     motorHome();                        //Initialise motor before any interrupt
     
     dt_I3.start();                      //Start the time counters for velocity
-        
+    
     controlTicker.attach(&control, dt);
     
     I1.rise(&i_edge);                   //Assign interrupt handlers for LEDs
@@ -99,6 +100,7 @@
     I2.fall(&i_edge);
     I3.rise(&i3rise);
     I3.fall(&i_edge);
+    
 //    CHA.rise(&CHA_rise);
 //    CHA.fall(&CHA_fall);
 //    CHB.rise(&CHB_rise);
@@ -107,7 +109,15 @@
     state = updateState();
     motorTimer.start();
     motorOut((state-orState+lead+6)%6, 1.0f);            //Kickstart the motor
-    
+    /*
+    while (motorTimer.read() < 30) {
+        motorOut((state-orState+1+6)%6, 0.75f);
+        wait(0.002);
+        motorOut((state-orState-1+6)%6, 0.75f);
+        wait(0.002);
+    }
+    stopMotor();  
+    */ 
     while (count_i3 <= goalRevs) {
 
         pc.printf("Speed: %f, goal speed: %f, duty cycle: %f, revs done: %d \n\r",w3, goalW, duty, count_i3);
@@ -123,8 +133,9 @@
         }
         */
     }
-    while (abs(w3) > 50){
+    while (abs(w3) > 40){
         pc.printf("Speed: %f, goal speed: %f, duty cycle: %f, revs done: %d \n\r",w3, goalW, duty, count_i3);
     }
+    stopMotor();
     return 0;
 }
\ No newline at end of file