Diff: main.cpp

Revision:

42:2ee563cd6223

Parent:

41:d52445129908

Child:

43:8b6b4040e635

diff -r d52445129908 -r 2ee563cd6223 main.cpp
--- a/main.cpp	Fri Mar 17 22:27:14 2017 +0000
+++ b/main.cpp	Fri Mar 17 23:17:42 2017 +0000
@@ -14,36 +14,36 @@
 volatile int CH_state = 0x00;
 volatile int CH_state_prev = 0x00;
 
-volatile float diskPosition = 0.0;  //in degrees
+volatile float diskPosition = 0.0;          //in degrees
 
 Timer dt_I3;
 Timer motorTimer;
 Ticker controlTicker;
 
-volatile float currentRevs = 0.0;                 //number of revs done
-volatile float goalRevs = 10.0;
-volatile float prevError = 0.0;
-volatile double dError = 0.0;
-volatile float currentError = 0.0;
+volatile float currentRevs = 0.0;           //number of revs done
+volatile float goalRevs = 10.0;             //number of revs to do
+volatile float prevError = 0.0;             //previous error in position
+volatile double dError = 0.0;               
+volatile float currentError = 0.0;          //current error in position
 
 #define kp 0.012f
-#define kd 0.02f //0.5f
+#define kd 0.02f
 #define k 10.0f
-#define dt 0.002f                        //given in ms, used to call the PID c.
+#define dt 0.002f                           //given in ms,used to call the PD c.
 
 void control(){
-    if (w3 > 300) {
-        lead = 2;
+    if (w3 > 300) {                         //restrict the motor speed to 300
+        lead = 2;                           // rad/s
         return;
     }
     prevError = currentError;
-    currentRevs = diskPosition / 360 + count_i3;    // 1/360degr + 2pi*revs
-    currentError = goalRevs - currentRevs;
-    dError = (currentError - prevError)/dt;
-    duty = k*(kp*currentError + kd*dError);
+    currentRevs = diskPosition / 360 + count_i3;        // angle/360 + #of revs
+    currentError = goalRevs - currentRevs;              //P term
+    dError = (currentError - prevError)/dt;             //D term
+    duty = k*(kp*currentError + kd*dError);             //Control motor duty
     if (duty > 0) lead = -2;
-    else {
-        lead = 2;
+    else {                                  //if duty < 0, reverse motor spin           
+        lead = 2;                           // direction to decelerate
         duty = -duty;
     }
 }
@@ -52,26 +52,25 @@
     state = updateState();
     motorOut((state-orState+lead+6)%6, duty);
     
-    if (I3.read() == 1) {
-        count_i3++;
+    if (I3.read() == 1) {                   //Only count revolutions if the
+        count_i3++;                         // rotor spins forward
     }    
 }
-
+//TODO merge with i_edge by measuring angular velocity in i1rise.
 void i3rise(){
     state = updateState();
     motorOut((state-orState+lead+6)%6, duty);
     
-    w3 = angle/dt_I3.read();            //Calc angular velocity
+    w3 = angle/dt_I3.read();                //Calc angular velocity
     
     dt_I3.reset();
-    //count_i3++;
 }
 
-void i_edge(){
-   state = updateState();
+void i_edge(){                              //Upon status led interrupt, update
+   state = updateState();                   // the motor output
    motorOut((state-orState+lead+6)%6, duty);
 }
-
+//Todo: add comments on this fucntion
 void updateDiskPosition() {
   if (CH_state != CH_state_prev) {
     int diff = CH_state - CH_state_prev;
@@ -128,7 +127,7 @@
     
     dt_I3.start();                      //Start the time counters for velocity
 
-    controlTicker.attach(&control, dt);
+    controlTicker.attach(&control, dt); //Adjust position every 2ms
     
     I1.rise(&i1rise);                   //Assign interrupt handlers for LEDs
     I1.fall(&i_edge);
@@ -137,19 +136,18 @@
     I3.rise(&i3rise);
     I3.fall(&i_edge);
     
-    CHA.rise(&CHA_rise);
-    CHA.fall(&CHA_fall);
+    CHA.rise(&CHA_rise);                //Assign interrupt handlers for
+    CHA.fall(&CHA_fall);                // precision angle LEDs
     CHB.rise(&CHB_rise);
     CHB.fall(&CHB_fall);
 
-    state = updateState();
+    state = updateState();              //Kickstart the motor
     motorTimer.start();
-    motorOut((state-orState+lead+6)%6, 0.3f);            //Kickstart the motor
+    motorOut((state-orState+lead+6)%6, 0.3f);
+    
     while (count_i3 <= goalRevs+1) {
-        //pc.printf("Speed: %f, duty cycle: %f, revs done: %d \n\r",w3, duty, count_i3);
         pc.printf("Speed: %f, duty cycle: %f, revs done: %d, dError: %f , currentError: %f, prevError: %f, currentRevs: %f \n\r",w3, duty, count_i3, dError, currentError, prevError, currentRevs);
-        //pc.printf("Disk position: %f \n\r",fi0);
     }
-    stopMotor();
+    stopMotor();                        //Stop the motor if position is reached
     return 0;
 }
\ No newline at end of file