Diff: main.cpp

Revision:

11:043a63c952a0

Parent:

10:25d8696cb2c6

Child:

13:deb1e793f125

Child:

16:d4948633c559

--- a/main.cpp	Tue Mar 14 17:38:46 2017 +0000
+++ b/main.cpp	Tue Mar 14 21:50:17 2017 +0000
@@ -9,30 +9,43 @@
 #define dt 0.02f //given in ms, used to call a ticker
 
 volatile uint8_t state = 0;
-volatile uint8_t orState = 0;                   //Motor rotor offset.
-volatile float w [3] = {0, 0, 0};               //Angular velocities
-volatile float avgW = 0;
-volatile float duty = 0.28;
+//volatile uint8_t orState = 0;                   //Motor rotor offset.
+volatile float w3 = 0;               //Angular velocity
+volatile float duty = 0.40;
 volatile int count_i3 = 0;
 const float angularVelocities[17] = {0, 112.355598, 164.975998, 218.721725,
 260.672943, 291.491364, 308.479126, 316.805908, 321.183929, 324.010712, 
 326.146759, 336.187103, 351.175629, 364.887604, 377.856659, 387.58432, 
 392.540314};
+const float dutyCycles [17] = {0, 0.28, 0.33, 0.38, 0.42, 0.47, 0.52, 0.57,
+                            0.63, 0.68, 0.73, 0.78, 0.83, 0.88, 0.93, 0.98, 1};
 
 const float angle = 6.283;                    //2*pi for 1 revolution
-Timer dt_I1;
-Timer dt_I2;
 Timer dt_I3;
 Timer motorTimer;
 Ticker controlTicker;
 
 volatile float fi0 = 0;     //number of revs done
-volatile int goalRevs = 20;
+volatile int goalRevs = 50;
 volatile float fi = 2*3.1415*goalRevs;
 volatile float goalW = 0;      //desired angular velocity
 volatile float accError = 0;
 volatile float prevError = 0;
 
+float getDuty(float w){
+    for (int i=0;i<16;i++) {    //iterate through the angular velocities
+        if (w > angularVelocities[i] && w <= angularVelocities[i+1]) {
+            if (w-angularVelocities[i] < angularVelocities[i+1]-w ) {
+                return dutyCycles[i];
+            }
+            else {
+                return dutyCycles[i+1];
+            }
+        }
+    }
+    return 0;
+}
+
 void control(){
     fi0 = 6.283 * count_i3; //fi0 = 2*pi*revs
     float error = fi - fi0;
@@ -40,36 +53,20 @@
     float dError = (prevError - error)/dt;
     goalW = kp*error + ki*accError + kd*dError;
     prevError = error;
-}
-
-
-void i1rise(){
-    state = updateState();
-    motorOut((state-orState+lead+6)%6, duty);
-    
-    w[0] = angle/dt_I1.read();                //Calc angular velocity
-    dt_I1.reset();                       
-}
-
-void i2rise(){
-    state = updateState();
-    motorOut((state-orState+lead+6)%6, duty);
-    
-    w[1] = angle/dt_I2.read();
-    dt_I2.reset();                       
+    duty = getDuty(goalW);
 }
 
 void i3rise(){
     state = updateState();
     motorOut((state-orState+lead+6)%6, duty);
     
-    w[2] = angle/dt_I3.read();
+    w3 = angle/dt_I3.read();      //Calc angular velocity
     
     dt_I3.reset();
     count_i3++;                   
 }
 
-void i_fall(){
+void i_edge(){
    state = updateState();
    motorOut((state-orState+lead+6)%6, duty);
 }  
@@ -84,42 +81,38 @@
 }
 
 int main() {
-    //Probably measure orState from hardware and make it a const?
-    orState = motorHome();  //Initialise motor before any interrupt
-    pc.printf("orState: %d \n\r", orState);
+    motorHome();  //Initialise motor before any interrupt
     
-    dt_I1.start();          //Start the time counters for velocity
-    dt_I2.start();          //Probably put these in an init function?
-    dt_I3.start();
+    dt_I3.start();          //Start the time counters for velocity
+        
+    //controlTicker.attach(&control, dt);
     
-    motorTimer.start();
-    
-    controlTicker.attach(&control, dt);
-    
-    I1.rise(&i1rise);       //Assign interrupt handlers for LEDs
-    I1.fall(&i_fall);
-    I2.rise(&i2rise);
-    I2.fall(&i_fall);
+    I1.rise(&i_edge);       //Assign interrupt handlers for LEDs
+    I1.fall(&i_edge);
+    I2.rise(&i_edge);
+    I2.fall(&i_edge);
     I3.rise(&i3rise);
-    I3.fall(&i_fall);
+    I3.fall(&i_edge);
 //    CHA.rise(&CHA_rise);
 //    CHA.fall(&CHA_fall);
 //    CHB.rise(&CHB_rise);
 //    CHB.fall(&CHB_fall);
+
     state = updateState();
+    motorTimer.start();
     motorOut((state-orState+lead+6)%6, 0.5f);            //Kickstart the motor
-    wait(30);
+    wait(60);
     
-    while (1) {
-        avgW = (w[0] + w[1] + w[2])/3;    //average speeds for better prediction
-        pc.printf("Speed: %f, duty cycle: %f  \n\r",avgW, duty);
-        
-        duty += 0.05f;
-        wait(10);
-        if(duty > 1.05f) {
+    while (1/*count_i3<=goalRevs*/) {
+        pc.printf("Speed: %f, duty cycle: %f, revs done: %d \n\r",w3, duty, count_i3);
+        stopMotor();
+        return 0;
+        /*
+        if(duty < 0.00f) {
             stopMotor();
             return 0;    
         }
+        */
         /*
         if(motorTimer.read() >= 30) {
             stopMotor();