Diff: main.cpp

Revision:

10:25d8696cb2c6

Parent:

7:6bf4a61cf7c7

Child:

11:043a63c952a0

diff -r 0d5b5398ddf5 -r 25d8696cb2c6 main.cpp
--- a/main.cpp	Tue Mar 14 14:16:55 2017 +0000
+++ b/main.cpp	Tue Mar 14 17:38:46 2017 +0000
@@ -3,19 +3,23 @@
 #include "definitions.h"
 #include "motorControl.h"
 
-#define kp 0.75
-#define ki 0.5
-#define kd 1
-#define dt 0.02 //given in ms, used to call a ticker
+#define kp 0.75f
+#define ki 0.5f
+#define kd 1.0f
+#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.75;
+volatile float duty = 0.28;
 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 uint16_t angle = 6283;                    //2*pi*1000 for 1 revolution
+const float angle = 6.283;                    //2*pi for 1 revolution
 Timer dt_I1;
 Timer dt_I2;
 Timer dt_I3;
@@ -26,49 +30,42 @@
 volatile int goalRevs = 20;
 volatile float fi = 2*3.1415*goalRevs;
 volatile float goalW = 0;      //desired angular velocity
-volatile int accError = 0;
+volatile float accError = 0;
+volatile float prevError = 0;
 
-/*
 void control(){
     fi0 = 6.283 * count_i3; //fi0 = 2*pi*revs
-    int error = fi - fi0;
-    accError += error;
-    dError = (prevError - error)/dt;    //prev error needs to be stored
+    float error = fi - fi0;
+    accError += error*dt;
+    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);
     
-    dt_I1.stop();
-    w[0] = angle/dt_I1.read_ms();                //Calc angular velocity
-    
-    dt_I1.reset();
-    dt_I1.start();                       
+    w[0] = angle/dt_I1.read();                //Calc angular velocity
+    dt_I1.reset();                       
 }
 
 void i2rise(){
     state = updateState();
     motorOut((state-orState+lead+6)%6, duty);
     
-    dt_I2.stop();
-    w[1] = angle/dt_I2.read_ms();
-    
-    dt_I2.reset();
-    dt_I2.start();                       
+    w[1] = angle/dt_I2.read();
+    dt_I2.reset();                       
 }
 
 void i3rise(){
     state = updateState();
     motorOut((state-orState+lead+6)%6, duty);
     
-    dt_I3.stop();
-    w[2] = angle/dt_I3.read_ms();
+    w[2] = angle/dt_I3.read();
     
     dt_I3.reset();
-    dt_I3.start();
     count_i3++;                   
 }
 
@@ -89,15 +86,15 @@
 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);
     
     dt_I1.start();          //Start the time counters for velocity
     dt_I2.start();          //Probably put these in an init function?
     dt_I3.start();
     
-    motorOut(4, duty);            //Kickstart the motor
     motorTimer.start();
     
-    //controlTicker.attach(&control, dt);
+    controlTicker.attach(&control, dt);
     
     I1.rise(&i1rise);       //Assign interrupt handlers for LEDs
     I1.fall(&i_fall);
@@ -109,30 +106,25 @@
 //    CHA.fall(&CHA_fall);
 //    CHB.rise(&CHB_rise);
 //    CHB.fall(&CHB_fall);
-
+    state = updateState();
+    motorOut((state-orState+lead+6)%6, 0.5f);            //Kickstart the motor
+    wait(30);
+    
     while (1) {
-        /*
-        const int rotations = 10;
-        pc.printf("Rotation: %d ",count_i3);
-        if (count_i3 == rotations) {
+        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) {
             stopMotor();
-            return 0;
+            return 0;    
         }
-        */
-        
-        avgW = (w[0] + w[1] + w[2])/3;    //average speeds for better prediction
-        pc.printf("Speed: %f, duty cycle: %f  \n\r",w[2], duty);
         /*
-        duty ++;
-        wait(2);
-        if(duty > 100) {
+        if(motorTimer.read() >= 30) {
             stopMotor();
             return 0;    
         }
         */
-        if(motorTimer.read_ms() >= 30000) {
-            stopMotor();
-            return 0;    
-        }
     }
 }
\ No newline at end of file