Revision 3:0d98aaab6345, committed 2020-04-28

Comitter:

tommyzhu19951204

Date:

Tue Apr 28 23:49:24 2020 +0000

Parent:

1:499f4e873fb2

Commit message:

v1.2; Adding comment and functionality

Changed in this revision

--- a/main.cpp	Sun Apr 26 18:59:42 2020 +0000
+++ b/main.cpp	Tue Apr 28 23:49:24 2020 +0000
@@ -1,54 +1,58 @@
 /*
-  Rotary Encoder Test
-  Demonstrates operation of Rotary Encoder
-  Displays results on Serial Monitor
+  ECE 4180 Final Project
+  Yatong Bai
+  
+  Functionalities:
+  Reads target speed from the serial port;
+  Uses an encoder with 11 ticks to calculate heading and angular velocity;
+  Uses a PID control algorithm to make the motor spin at the target speed;
+  Estimate the heading between two encoder ticks;
+  Lights up LED1 when heading is ~0 degrees
 */
 
 #include "mbed.h"
 
-#define tick 32.727272727272727
+#define tick 32.727272727272727 // 360 deg / 11 ticks
 #define pi 3.14159265358979323846264338
 
 InterruptIn encoder (p22);
 PwmOut motor (p21);
 Serial pc (USBTX, USBRX);
-DigitalOut led(LED1);
-DigitalOut blinker(LED2);
-Timer t;
-Timer t2;
-Ticker ticker;
+DigitalOut led(LED1); // LED1 blinks when heading is ~0 deg
+DigitalOut blinker(LED2); // LED2 toggles when the interrupt is run
+Timer t; // Used to calculate the time interval between two rising edges
+Ticker ticker; // For timer interrupt
 
 volatile double rev = 0, old_rev = 0, raw_rev = 0, print_rev = 0;
 volatile double elapsed_time = 99999999999, cur_elapsed_time = 0;
 volatile double angle = 0, raw_angle = 0, target = 0;
 volatile double cur_angle = 0, print_angle = 0, num = 0, err = 0;
 
-volatile int p = 0, i = 0, d = 0, spd = 0, up_lim = 255;
-volatile int cntr = 0, print_spd = 0;
+volatile float p = 0, i = 0, d = 0, spd = 0, up_lim = 255;
 char buf[5];
 
-void PID(){
-    // PID
+void PID(){ // Helper function that performs PID control
     err = target-rev;
-    p = err*(110*target+500) + target*5 + 40;
-    double temp = err*elapsed_time*(target*330+110);
-    if (temp < 20 && temp > -20)
-        i += temp;
-    else if (temp >= 20)
+    p = err*(110*target+500) + target*5 + 40; // P term
+    double temp = err*elapsed_time*(target*330+110); // I term increment
+    if (temp >= 20) // Prevent abnormal data upsetting the I term
         i += 20;
+    else if (temp <= -20)
+        i -= 20;
     else
-        i -= 20;
-    if (i < -75)
-        i = -75;
-    else if (i > 100+target*70)
-        i = 100 + target * 70;
-    d = (old_rev-rev)/elapsed_time*7;
-    spd = p + i + d;
+        i += temp;
+    if (i < -65) // I term should generally be positive
+        i = -65;
+    else if (i > 110+target*70) // Limit max I to prevent wind-up during acceleration
+        i = 110 + target * 70;
+    d = (old_rev-rev)/elapsed_time*7; // D term
+    spd = p + i + d; // PID controller
   
-    if (target == 0)
+    if (target == 0) // Set speed to 0 when target speed = 0
         spd = 0;
     
     else{
+         // Limit the PWM pulse width to prevent large current at low speed
         up_lim = target * 25 + 160;
     
         if (spd < 0)
@@ -61,102 +65,100 @@
         }
     }
     
-    motor = spd / 255.0;
+    motor = spd / 255.0; // Set PWM output
 }
 
-void control() {
+void control() { // Interrupt function called when encoder generates a rising edge
     t.stop();
-    elapsed_time = t.read();
-    t.reset();
-    t.start();
+    elapsed_time = t.read(); // Calculate the time interval between two rising edges
+    t.reset(); // Reset timer
+    t.start(); // Start timer again
     
-    blinker = !blinker;
+    blinker = !blinker; // Toggle LED2
   
-    if (elapsed_time == 0)
+    if (elapsed_time == 0) // Prevent dividing by 0 when this function is called for the first time
         raw_rev = 7;
     else
-        raw_rev = tick/360.0/elapsed_time;
+        raw_rev = tick/360.0/elapsed_time; // Calculate angular velocity
     rev = raw_rev;
   
-    // Discard unrealistic revs
-    if (rev - 1.5*print_rev > 1.3 || rev > 6){
+    // Discard unrealistic rev readings caused by rising edges that are too close (debouncing)
+    if (rev - 1.5*print_rev > 1.3 || rev > 6)
         rev = print_rev;
-        cntr++;
-    }
     else{
-        old_rev = rev;
-        raw_angle += tick;
-        angle = (raw_angle + 2*cur_angle) / 3.;
+        old_rev = rev; // old_rev will be used for the D term of PID
+        raw_angle += tick; // Increment the angle
+        // Estimate the current angle based on angular velocity and tick increment
+        angle = (raw_angle + 2.0*cur_angle) / 3.0; 
     }
-    if (rev<6.5)
-        print_rev = (print_rev * 29. + rev) / 30.;
+    if (rev < 6.5) // Apply a low-pass filter on the angular velocity reading for display
+        print_rev = (print_rev * 29.0 + rev) / 30.0;
   
-    PID();
+    PID(); // Call the PID helper function to perform control
 }
 
-void output(){
+void output(){ // A timer interrupt function that sends the data to the PC
     pc.printf("Heading: %3.0f deg, Speed: %3.1f revs/sec\n", print_angle, print_rev);
 }
 
-int main() {
-    encoder.rise(&control);
-    pc.baud(38400);
-    pc.printf("Hello\n");
-    motor.period_us(800);
-    t2.reset();
-    t2.start();
-    ticker.attach(&output, 0.1);
+int main() { // Main function
+    pc.baud(38400); // Jack up PC baud rate
+    motor.period_us(800); // Set PWM frequency to 1250Hz
+    t.reset(); // Reset and start the timers
+    t.start();
+    encoder.rise(&control); // Attach the I/O interrupt function
+    ticker.attach(&output, 0.08); // Attach the timer interrupt function
   
     while (true){
-  
         if (pc.readable()){
-            pc.scanf("%s", &buf);
-            num = atof(buf);
-            if (num > 6){
+            pc.scanf("%s", &buf); // Read data from PC
+            num = atof(buf); // Convert string to double
+            if (num > 6){ // A number > 6 is a signal for resetting the heading to 0
                 angle -= cur_angle;
                 raw_angle -= cur_angle;
                 cur_angle = 0;
                 elapsed_time = 99999999999;
                 rev = 0;
             }
-            else{
-                target = num * 1.005;
-                motor = (target*40.0+50.0) / 255.0;
-                i = 0;
+            else{ // Otherwise the number is the target speed
+                target = num; // Set target speed
+                motor = (target*40.0+50.0) / 255.0; // Set PWM pulse width to ease startup
+                i = 0; // Clear the I term
             }
         }
-        
-        if (t2 > 6){
-            t2.stop();
-            t2.reset();
-            t2.start();
-            i = 50;
-        }
 
+        // Since the encoder has poor accuracy, the heading needs to be estimated between two edges
+        // Keep reading from the timer to calculate the elapsed time since last rising edge
+        // Calculate elapsed angle since last rising edge using the angular velocity
         cur_elapsed_time = t.read();
-        if ((target<5 && cur_elapsed_time>elapsed_time) || cur_elapsed_time>1){
-            rev = tick/360.0/cur_elapsed_time;
-            if (rev - 1.2*print_rev > 0.2 || rev > 5)
-                rev = print_rev;
-            PID();
-        }
-       
-        cur_angle = angle + print_rev*360*cur_elapsed_time;
-        if (cur_angle > angle + tick)
+        cur_angle = angle + print_rev*360*cur_elapsed_time; // Estimate current angle
+        if (cur_angle > angle + tick) // The estimated angle cannot exceed 1 tick
             cur_angle = angle + tick;
-        if (cur_angle > 360.){
+        if (cur_angle > 360.){ // Reset angle when the next turn begins
             raw_angle -= 360.;
             angle -= 360.;
             cur_angle -= 360.;
         }
+        
+        // If the interval between two edges is too long (longer than the previous interval)
+        // Then it means the speed has reduced. Need to re-calculate the speed.
+        // This is especially during start-up. If stuck, the PWM pulse width will continuously increase
+        if ((target<5 && cur_elapsed_time>elapsed_time) || cur_elapsed_time>1){
+            rev = tick/360.0/cur_elapsed_time;
+            if (rev - 1.2*print_rev > 0.2 || rev > 5) // Discard unrealistic revs
+                rev = print_rev;
+            PID(); // Perform PID control
+        }
     
-        if (rev < 6.5){
+        if (rev < 6.5){ // Apply low-pass filter on rev
             print_rev = (print_rev * 349. + rev) / 350.;
             print_angle = cur_angle;
-            if (print_angle < 10 || print_angle > 350)
-                led = 1;
-            else
-                led = 0;
         }
+        
+        // Light up the LED when heading is ~0 deg
+        if (cur_angle < 10 || cur_angle > 350)
+            led = 1;
+        else
+            led = 0;
     }
 }
\ No newline at end of file