ECE4180_Avionic System Test Stand / Mbed 2 deprecated test_stand

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

main.cpp@3:0d98aaab6345, 2020-04-28 (annotated)

Committer:
tommyzhu19951204
Date:
Tue Apr 28 23:49:24 2020 +0000
Revision:
3:0d98aaab6345
Parent:
1:499f4e873fb2 
v1.2; Adding comment and functionality

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tommyzhu19951204 0:5252dc3a058e 1 /*
tommyzhu19951204 3:0d98aaab6345 2 ECE 4180 Final Project
tommyzhu19951204 3:0d98aaab6345 3 Yatong Bai
tommyzhu19951204 3:0d98aaab6345 4
tommyzhu19951204 3:0d98aaab6345 5 Functionalities:
tommyzhu19951204 3:0d98aaab6345 6 Reads target speed from the serial port;
tommyzhu19951204 3:0d98aaab6345 7 Uses an encoder with 11 ticks to calculate heading and angular velocity;
tommyzhu19951204 3:0d98aaab6345 8 Uses a PID control algorithm to make the motor spin at the target speed;
tommyzhu19951204 3:0d98aaab6345 9 Estimate the heading between two encoder ticks;
tommyzhu19951204 3:0d98aaab6345 10 Lights up LED1 when heading is ~0 degrees
tommyzhu19951204 0:5252dc3a058e 11 */
tommyzhu19951204 0:5252dc3a058e 12
tommyzhu19951204 0:5252dc3a058e 13 #include "mbed.h"
tommyzhu19951204 0:5252dc3a058e 14
tommyzhu19951204 3:0d98aaab6345 15 #define tick 32.727272727272727 // 360 deg / 11 ticks
tommyzhu19951204 0:5252dc3a058e 16 #define pi 3.14159265358979323846264338
tommyzhu19951204 0:5252dc3a058e 17
tommyzhu19951204 0:5252dc3a058e 18 InterruptIn encoder (p22);
tommyzhu19951204 0:5252dc3a058e 19 PwmOut motor (p21);
tommyzhu19951204 0:5252dc3a058e 20 Serial pc (USBTX, USBRX);
tommyzhu19951204 3:0d98aaab6345 21 DigitalOut led(LED1); // LED1 blinks when heading is ~0 deg
tommyzhu19951204 3:0d98aaab6345 22 DigitalOut blinker(LED2); // LED2 toggles when the interrupt is run
tommyzhu19951204 3:0d98aaab6345 23 Timer t; // Used to calculate the time interval between two rising edges
tommyzhu19951204 3:0d98aaab6345 24 Ticker ticker; // For timer interrupt
tommyzhu19951204 0:5252dc3a058e 25
tommyzhu19951204 0:5252dc3a058e 26 volatile double rev = 0, old_rev = 0, raw_rev = 0, print_rev = 0;
tommyzhu19951204 0:5252dc3a058e 27 volatile double elapsed_time = 99999999999, cur_elapsed_time = 0;
tommyzhu19951204 1:499f4e873fb2 28 volatile double angle = 0, raw_angle = 0, target = 0;
tommyzhu19951204 0:5252dc3a058e 29 volatile double cur_angle = 0, print_angle = 0, num = 0, err = 0;
tommyzhu19951204 0:5252dc3a058e 30
tommyzhu19951204 3:0d98aaab6345 31 volatile float p = 0, i = 0, d = 0, spd = 0, up_lim = 255;
tommyzhu19951204 0:5252dc3a058e 32 char buf[5];
tommyzhu19951204 0:5252dc3a058e 33
tommyzhu19951204 3:0d98aaab6345 34 void PID(){ // Helper function that performs PID control
tommyzhu19951204 0:5252dc3a058e 35 err = target-rev;
tommyzhu19951204 3:0d98aaab6345 36 p = err*(110*target+500) + target*5 + 40; // P term
tommyzhu19951204 3:0d98aaab6345 37 double temp = err*elapsed_time*(target*330+110); // I term increment
tommyzhu19951204 3:0d98aaab6345 38 if (temp >= 20) // Prevent abnormal data upsetting the I term
tommyzhu19951204 0:5252dc3a058e 39 i += 20;
tommyzhu19951204 3:0d98aaab6345 40 else if (temp <= -20)
tommyzhu19951204 3:0d98aaab6345 41 i -= 20;
tommyzhu19951204 0:5252dc3a058e 42 else
tommyzhu19951204 3:0d98aaab6345 43 i += temp;
tommyzhu19951204 3:0d98aaab6345 44 if (i < -65) // I term should generally be positive
tommyzhu19951204 3:0d98aaab6345 45 i = -65;
tommyzhu19951204 3:0d98aaab6345 46 else if (i > 110+target*70) // Limit max I to prevent wind-up during acceleration
tommyzhu19951204 3:0d98aaab6345 47 i = 110 + target * 70;
tommyzhu19951204 3:0d98aaab6345 48 d = (old_rev-rev)/elapsed_time*7; // D term
tommyzhu19951204 3:0d98aaab6345 49 spd = p + i + d; // PID controller
tommyzhu19951204 0:5252dc3a058e 50
tommyzhu19951204 3:0d98aaab6345 51 if (target == 0) // Set speed to 0 when target speed = 0
tommyzhu19951204 0:5252dc3a058e 52 spd = 0;
tommyzhu19951204 0:5252dc3a058e 53
tommyzhu19951204 0:5252dc3a058e 54 else{
tommyzhu19951204 3:0d98aaab6345 55 // Limit the PWM pulse width to prevent large current at low speed
tommyzhu19951204 0:5252dc3a058e 56 up_lim = target * 25 + 160;
tommyzhu19951204 0:5252dc3a058e 57
tommyzhu19951204 1:499f4e873fb2 58 if (spd < 0)
tommyzhu19951204 1:499f4e873fb2 59 spd = 0;
tommyzhu19951204 0:5252dc3a058e 60 else{
tommyzhu19951204 0:5252dc3a058e 61 if (spd > up_lim)
tommyzhu19951204 0:5252dc3a058e 62 spd = up_lim;
tommyzhu19951204 0:5252dc3a058e 63 if (spd > 255)
tommyzhu19951204 0:5252dc3a058e 64 spd = 255;
tommyzhu19951204 0:5252dc3a058e 65 }
tommyzhu19951204 0:5252dc3a058e 66 }
tommyzhu19951204 0:5252dc3a058e 67
tommyzhu19951204 3:0d98aaab6345 68 motor = spd / 255.0; // Set PWM output
tommyzhu19951204 0:5252dc3a058e 69 }
tommyzhu19951204 0:5252dc3a058e 70
tommyzhu19951204 3:0d98aaab6345 71 void control() { // Interrupt function called when encoder generates a rising edge
tommyzhu19951204 0:5252dc3a058e 72 t.stop();
tommyzhu19951204 3:0d98aaab6345 73 elapsed_time = t.read(); // Calculate the time interval between two rising edges
tommyzhu19951204 3:0d98aaab6345 74 t.reset(); // Reset timer
tommyzhu19951204 3:0d98aaab6345 75 t.start(); // Start timer again
tommyzhu19951204 0:5252dc3a058e 76
tommyzhu19951204 3:0d98aaab6345 77 blinker = !blinker; // Toggle LED2
tommyzhu19951204 0:5252dc3a058e 78
tommyzhu19951204 3:0d98aaab6345 79 if (elapsed_time == 0) // Prevent dividing by 0 when this function is called for the first time
tommyzhu19951204 0:5252dc3a058e 80 raw_rev = 7;
tommyzhu19951204 0:5252dc3a058e 81 else
tommyzhu19951204 3:0d98aaab6345 82 raw_rev = tick/360.0/elapsed_time; // Calculate angular velocity
tommyzhu19951204 0:5252dc3a058e 83 rev = raw_rev;
tommyzhu19951204 0:5252dc3a058e 84
tommyzhu19951204 3:0d98aaab6345 85 // Discard unrealistic rev readings caused by rising edges that are too close (debouncing)
tommyzhu19951204 3:0d98aaab6345 86 if (rev - 1.5*print_rev > 1.3 || rev > 6)
tommyzhu19951204 0:5252dc3a058e 87 rev = print_rev;
tommyzhu19951204 0:5252dc3a058e 88 else{
tommyzhu19951204 3:0d98aaab6345 89 old_rev = rev; // old_rev will be used for the D term of PID
tommyzhu19951204 3:0d98aaab6345 90 raw_angle += tick; // Increment the angle
tommyzhu19951204 3:0d98aaab6345 91 // Estimate the current angle based on angular velocity and tick increment
tommyzhu19951204 3:0d98aaab6345 92 angle = (raw_angle + 2.0*cur_angle) / 3.0;
tommyzhu19951204 0:5252dc3a058e 93 }
tommyzhu19951204 3:0d98aaab6345 94 if (rev < 6.5) // Apply a low-pass filter on the angular velocity reading for display
tommyzhu19951204 3:0d98aaab6345 95 print_rev = (print_rev * 29.0 + rev) / 30.0;
tommyzhu19951204 0:5252dc3a058e 96
tommyzhu19951204 3:0d98aaab6345 97 PID(); // Call the PID helper function to perform control
tommyzhu19951204 0:5252dc3a058e 98 }
tommyzhu19951204 0:5252dc3a058e 99
tommyzhu19951204 3:0d98aaab6345 100 void output(){ // A timer interrupt function that sends the data to the PC
tommyzhu19951204 1:499f4e873fb2 101 pc.printf("Heading: %3.0f deg, Speed: %3.1f revs/sec\n", print_angle, print_rev);
tommyzhu19951204 1:499f4e873fb2 102 }
tommyzhu19951204 1:499f4e873fb2 103
tommyzhu19951204 3:0d98aaab6345 104 int main() { // Main function
tommyzhu19951204 3:0d98aaab6345 105 pc.baud(38400); // Jack up PC baud rate
tommyzhu19951204 3:0d98aaab6345 106 motor.period_us(800); // Set PWM frequency to 1250Hz
tommyzhu19951204 3:0d98aaab6345 107 t.reset(); // Reset and start the timers
tommyzhu19951204 3:0d98aaab6345 108 t.start();
tommyzhu19951204 3:0d98aaab6345 109 encoder.rise(&control); // Attach the I/O interrupt function
tommyzhu19951204 3:0d98aaab6345 110 ticker.attach(&output, 0.08); // Attach the timer interrupt function
tommyzhu19951204 0:5252dc3a058e 111
tommyzhu19951204 0:5252dc3a058e 112 while (true){
tommyzhu19951204 0:5252dc3a058e 113 if (pc.readable()){
tommyzhu19951204 3:0d98aaab6345 114 pc.scanf("%s", &buf); // Read data from PC
tommyzhu19951204 3:0d98aaab6345 115 num = atof(buf); // Convert string to double
tommyzhu19951204 3:0d98aaab6345 116 if (num > 6){ // A number > 6 is a signal for resetting the heading to 0
tommyzhu19951204 0:5252dc3a058e 117 angle -= cur_angle;
tommyzhu19951204 0:5252dc3a058e 118 raw_angle -= cur_angle;
tommyzhu19951204 0:5252dc3a058e 119 cur_angle = 0;
tommyzhu19951204 0:5252dc3a058e 120 elapsed_time = 99999999999;
tommyzhu19951204 0:5252dc3a058e 121 rev = 0;
tommyzhu19951204 0:5252dc3a058e 122 }
tommyzhu19951204 3:0d98aaab6345 123 else{ // Otherwise the number is the target speed
tommyzhu19951204 3:0d98aaab6345 124 target = num; // Set target speed
tommyzhu19951204 3:0d98aaab6345 125 motor = (target*40.0+50.0) / 255.0; // Set PWM pulse width to ease startup
tommyzhu19951204 3:0d98aaab6345 126 i = 0; // Clear the I term
tommyzhu19951204 0:5252dc3a058e 127 }
tommyzhu19951204 0:5252dc3a058e 128 }
tommyzhu19951204 0:5252dc3a058e 129
tommyzhu19951204 3:0d98aaab6345 130 // Since the encoder has poor accuracy, the heading needs to be estimated between two edges
tommyzhu19951204 3:0d98aaab6345 131 // Keep reading from the timer to calculate the elapsed time since last rising edge
tommyzhu19951204 3:0d98aaab6345 132 // Calculate elapsed angle since last rising edge using the angular velocity
tommyzhu19951204 0:5252dc3a058e 133 cur_elapsed_time = t.read();
tommyzhu19951204 3:0d98aaab6345 134 cur_angle = angle + print_rev*360*cur_elapsed_time; // Estimate current angle
tommyzhu19951204 3:0d98aaab6345 135 if (cur_angle > angle + tick) // The estimated angle cannot exceed 1 tick
tommyzhu19951204 0:5252dc3a058e 136 cur_angle = angle + tick;
tommyzhu19951204 3:0d98aaab6345 137 if (cur_angle > 360.){ // Reset angle when the next turn begins
tommyzhu19951204 0:5252dc3a058e 138 raw_angle -= 360.;
tommyzhu19951204 0:5252dc3a058e 139 angle -= 360.;
tommyzhu19951204 0:5252dc3a058e 140 cur_angle -= 360.;
tommyzhu19951204 0:5252dc3a058e 141 }
tommyzhu19951204 3:0d98aaab6345 142
tommyzhu19951204 3:0d98aaab6345 143 // If the interval between two edges is too long (longer than the previous interval)
tommyzhu19951204 3:0d98aaab6345 144 // Then it means the speed has reduced. Need to re-calculate the speed.
tommyzhu19951204 3:0d98aaab6345 145 // This is especially during start-up. If stuck, the PWM pulse width will continuously increase
tommyzhu19951204 3:0d98aaab6345 146 if ((target<5 && cur_elapsed_time>elapsed_time) || cur_elapsed_time>1){
tommyzhu19951204 3:0d98aaab6345 147 rev = tick/360.0/cur_elapsed_time;
tommyzhu19951204 3:0d98aaab6345 148 if (rev - 1.2*print_rev > 0.2 || rev > 5) // Discard unrealistic revs
tommyzhu19951204 3:0d98aaab6345 149 rev = print_rev;
tommyzhu19951204 3:0d98aaab6345 150 PID(); // Perform PID control
tommyzhu19951204 3:0d98aaab6345 151 }
tommyzhu19951204 0:5252dc3a058e 152
tommyzhu19951204 3:0d98aaab6345 153 if (rev < 6.5){ // Apply low-pass filter on rev
tommyzhu19951204 1:499f4e873fb2 154 print_rev = (print_rev * 349. + rev) / 350.;
tommyzhu19951204 0:5252dc3a058e 155 print_angle = cur_angle;
tommyzhu19951204 0:5252dc3a058e 156 }
tommyzhu19951204 3:0d98aaab6345 157
tommyzhu19951204 3:0d98aaab6345 158 // Light up the LED when heading is ~0 deg
tommyzhu19951204 3:0d98aaab6345 159 if (cur_angle < 10 || cur_angle > 350)
tommyzhu19951204 3:0d98aaab6345 160 led = 1;
tommyzhu19951204 3:0d98aaab6345 161 else
tommyzhu19951204 3:0d98aaab6345 162 led = 0;
tommyzhu19951204 0:5252dc3a058e 163 }
tommyzhu19951204 0:5252dc3a058e 164 }