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@0:5252dc3a058e, 2020-04-21 (annotated)

Committer:
tommyzhu19951204
Date:
Tue Apr 21 15:19:37 2020 +0000
Revision:
0:5252dc3a058e
Child:
1:499f4e873fb2
v1.0

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tommyzhu19951204 0:5252dc3a058e 1 /*
tommyzhu19951204 0:5252dc3a058e 2 Rotary Encoder Test
tommyzhu19951204 0:5252dc3a058e 3 Demonstrates operation of Rotary Encoder
tommyzhu19951204 0:5252dc3a058e 4 Displays results on Serial Monitor
tommyzhu19951204 0:5252dc3a058e 5 */
tommyzhu19951204 0:5252dc3a058e 6
tommyzhu19951204 0:5252dc3a058e 7 #include "mbed.h"
tommyzhu19951204 0:5252dc3a058e 8
tommyzhu19951204 0:5252dc3a058e 9 #define tick 32.727272727272727
tommyzhu19951204 0:5252dc3a058e 10 #define pi 3.14159265358979323846264338
tommyzhu19951204 0:5252dc3a058e 11
tommyzhu19951204 0:5252dc3a058e 12 InterruptIn encoder (p22);
tommyzhu19951204 0:5252dc3a058e 13 PwmOut motor (p21);
tommyzhu19951204 0:5252dc3a058e 14 Serial pc (USBTX, USBRX);
tommyzhu19951204 0:5252dc3a058e 15 DigitalOut led(LED1);
tommyzhu19951204 0:5252dc3a058e 16 DigitalOut blinker(LED2);
tommyzhu19951204 0:5252dc3a058e 17 Timer t;
tommyzhu19951204 0:5252dc3a058e 18 Timer t2;
tommyzhu19951204 0:5252dc3a058e 19
tommyzhu19951204 0:5252dc3a058e 20 volatile double rev = 0, old_rev = 0, raw_rev = 0, print_rev = 0;
tommyzhu19951204 0:5252dc3a058e 21 volatile double elapsed_time = 99999999999, cur_elapsed_time = 0;
tommyzhu19951204 0:5252dc3a058e 22 volatile double angle = 0, raw_angle = 0, target = 0, target_diff = 0;
tommyzhu19951204 0:5252dc3a058e 23 volatile double cur_angle = 0, print_angle = 0, num = 0, err = 0;
tommyzhu19951204 0:5252dc3a058e 24
tommyzhu19951204 0:5252dc3a058e 25 volatile int p = 0, i = 0, d = 0, spd = 0, up_lim = 255;
tommyzhu19951204 0:5252dc3a058e 26 volatile int low_lim = 20, cntr = 0, print_spd = 0;
tommyzhu19951204 0:5252dc3a058e 27 char buf[5];
tommyzhu19951204 0:5252dc3a058e 28 volatile bool bypass = false, bypass2 = false;
tommyzhu19951204 0:5252dc3a058e 29
tommyzhu19951204 0:5252dc3a058e 30 void PID(){
tommyzhu19951204 0:5252dc3a058e 31 // PID
tommyzhu19951204 0:5252dc3a058e 32 err = target-rev;
tommyzhu19951204 0:5252dc3a058e 33 p = err*(110*target+500);
tommyzhu19951204 0:5252dc3a058e 34 double temp = err*elapsed_time*(target*330+110);
tommyzhu19951204 0:5252dc3a058e 35 if (temp < 20 && temp > -20)
tommyzhu19951204 0:5252dc3a058e 36 i += temp;
tommyzhu19951204 0:5252dc3a058e 37 else if (temp >= 20)
tommyzhu19951204 0:5252dc3a058e 38 i += 20;
tommyzhu19951204 0:5252dc3a058e 39 else
tommyzhu19951204 0:5252dc3a058e 40 i -= 20;
tommyzhu19951204 0:5252dc3a058e 41 if (i < -5)
tommyzhu19951204 0:5252dc3a058e 42 i = -5;
tommyzhu19951204 0:5252dc3a058e 43 else if (i > 120+target*80)
tommyzhu19951204 0:5252dc3a058e 44 i = 300;
tommyzhu19951204 0:5252dc3a058e 45 d = (old_rev-rev)/elapsed_time*7;
tommyzhu19951204 0:5252dc3a058e 46 spd = p + i + d;
tommyzhu19951204 0:5252dc3a058e 47
tommyzhu19951204 0:5252dc3a058e 48 if (target == 0)
tommyzhu19951204 0:5252dc3a058e 49 spd = 0;
tommyzhu19951204 0:5252dc3a058e 50
tommyzhu19951204 0:5252dc3a058e 51 else{
tommyzhu19951204 0:5252dc3a058e 52 up_lim = target * 25 + 160;
tommyzhu19951204 0:5252dc3a058e 53
tommyzhu19951204 0:5252dc3a058e 54 if (spd < low_lim)
tommyzhu19951204 0:5252dc3a058e 55 spd = low_lim;
tommyzhu19951204 0:5252dc3a058e 56 else{
tommyzhu19951204 0:5252dc3a058e 57 if (spd > up_lim)
tommyzhu19951204 0:5252dc3a058e 58 spd = up_lim;
tommyzhu19951204 0:5252dc3a058e 59 if (spd > 255)
tommyzhu19951204 0:5252dc3a058e 60 spd = 255;
tommyzhu19951204 0:5252dc3a058e 61 }
tommyzhu19951204 0:5252dc3a058e 62 }
tommyzhu19951204 0:5252dc3a058e 63
tommyzhu19951204 0:5252dc3a058e 64 motor = spd / 255.0;
tommyzhu19951204 0:5252dc3a058e 65 }
tommyzhu19951204 0:5252dc3a058e 66
tommyzhu19951204 0:5252dc3a058e 67 void control() {
tommyzhu19951204 0:5252dc3a058e 68 t.stop();
tommyzhu19951204 0:5252dc3a058e 69 elapsed_time = t.read();
tommyzhu19951204 0:5252dc3a058e 70 t.reset();
tommyzhu19951204 0:5252dc3a058e 71 t.start();
tommyzhu19951204 0:5252dc3a058e 72
tommyzhu19951204 0:5252dc3a058e 73 blinker = !blinker;
tommyzhu19951204 0:5252dc3a058e 74
tommyzhu19951204 0:5252dc3a058e 75 if (elapsed_time == 0)
tommyzhu19951204 0:5252dc3a058e 76 raw_rev = 7;
tommyzhu19951204 0:5252dc3a058e 77 else
tommyzhu19951204 0:5252dc3a058e 78 raw_rev = tick/360.0/elapsed_time;
tommyzhu19951204 0:5252dc3a058e 79 rev = raw_rev;
tommyzhu19951204 0:5252dc3a058e 80
tommyzhu19951204 0:5252dc3a058e 81 // Discard unrealistic revs
tommyzhu19951204 0:5252dc3a058e 82 if (rev - 1.3*print_rev > 1 || rev > 6){
tommyzhu19951204 0:5252dc3a058e 83 rev = print_rev;
tommyzhu19951204 0:5252dc3a058e 84 cntr++;
tommyzhu19951204 0:5252dc3a058e 85 }
tommyzhu19951204 0:5252dc3a058e 86 else{
tommyzhu19951204 0:5252dc3a058e 87 old_rev = rev;
tommyzhu19951204 0:5252dc3a058e 88 raw_angle += tick;
tommyzhu19951204 0:5252dc3a058e 89 angle = (raw_angle + 2*cur_angle) / 3.;
tommyzhu19951204 0:5252dc3a058e 90 }
tommyzhu19951204 0:5252dc3a058e 91 if (rev<6.5)
tommyzhu19951204 0:5252dc3a058e 92 print_rev = (print_rev * 39. + rev) / 40.;
tommyzhu19951204 0:5252dc3a058e 93
tommyzhu19951204 0:5252dc3a058e 94 PID();
tommyzhu19951204 0:5252dc3a058e 95 }
tommyzhu19951204 0:5252dc3a058e 96
tommyzhu19951204 0:5252dc3a058e 97 int main() {
tommyzhu19951204 0:5252dc3a058e 98 encoder.rise(&control);
tommyzhu19951204 0:5252dc3a058e 99 pc.baud(38400);
tommyzhu19951204 0:5252dc3a058e 100 pc.printf("Hello\n");
tommyzhu19951204 0:5252dc3a058e 101 motor.period_us(800);
tommyzhu19951204 0:5252dc3a058e 102
tommyzhu19951204 0:5252dc3a058e 103 while (true){
tommyzhu19951204 0:5252dc3a058e 104
tommyzhu19951204 0:5252dc3a058e 105 if (pc.readable()){
tommyzhu19951204 0:5252dc3a058e 106 pc.scanf("%s", &buf);
tommyzhu19951204 0:5252dc3a058e 107 num = atof(buf);
tommyzhu19951204 0:5252dc3a058e 108 if (num > 6){
tommyzhu19951204 0:5252dc3a058e 109 angle -= cur_angle;
tommyzhu19951204 0:5252dc3a058e 110 raw_angle -= cur_angle;
tommyzhu19951204 0:5252dc3a058e 111 cur_angle = 0;
tommyzhu19951204 0:5252dc3a058e 112 elapsed_time = 99999999999;
tommyzhu19951204 0:5252dc3a058e 113 rev = 0;
tommyzhu19951204 0:5252dc3a058e 114 }
tommyzhu19951204 0:5252dc3a058e 115 else{
tommyzhu19951204 0:5252dc3a058e 116 target_diff = abs(target-num);
tommyzhu19951204 0:5252dc3a058e 117 target = num * 1.005;
tommyzhu19951204 0:5252dc3a058e 118 motor = (target*40.0+50.0) / 255.0;
tommyzhu19951204 0:5252dc3a058e 119 bypass = true;
tommyzhu19951204 0:5252dc3a058e 120 i = 0;
tommyzhu19951204 0:5252dc3a058e 121 low_lim = 0;
tommyzhu19951204 0:5252dc3a058e 122 }
tommyzhu19951204 0:5252dc3a058e 123 }
tommyzhu19951204 0:5252dc3a058e 124
tommyzhu19951204 0:5252dc3a058e 125 cur_elapsed_time = t.read();
tommyzhu19951204 0:5252dc3a058e 126 if (target<2 && cur_elapsed_time>elapsed_time){
tommyzhu19951204 0:5252dc3a058e 127 rev = tick/360.0/cur_elapsed_time;
tommyzhu19951204 0:5252dc3a058e 128 if (rev - 1.2*print_rev > 0.2 || rev > 5)
tommyzhu19951204 0:5252dc3a058e 129 rev = print_rev;
tommyzhu19951204 0:5252dc3a058e 130 PID();
tommyzhu19951204 0:5252dc3a058e 131 }
tommyzhu19951204 0:5252dc3a058e 132
tommyzhu19951204 0:5252dc3a058e 133 cur_angle = angle + print_rev*360*cur_elapsed_time;
tommyzhu19951204 0:5252dc3a058e 134 if (cur_angle > angle + tick)
tommyzhu19951204 0:5252dc3a058e 135 cur_angle = angle + tick;
tommyzhu19951204 0:5252dc3a058e 136 if (cur_angle > 360.){
tommyzhu19951204 0:5252dc3a058e 137 raw_angle -= 360.;
tommyzhu19951204 0:5252dc3a058e 138 angle -= 360.;
tommyzhu19951204 0:5252dc3a058e 139 cur_angle -= 360.;
tommyzhu19951204 0:5252dc3a058e 140 }
tommyzhu19951204 0:5252dc3a058e 141
tommyzhu19951204 0:5252dc3a058e 142
tommyzhu19951204 0:5252dc3a058e 143 if (rev < 6.5){
tommyzhu19951204 0:5252dc3a058e 144 print_rev = (print_rev * 39. + rev) / 40.;
tommyzhu19951204 0:5252dc3a058e 145 print_angle = cur_angle;
tommyzhu19951204 0:5252dc3a058e 146 if (print_angle < 10 || print_angle > 350)
tommyzhu19951204 0:5252dc3a058e 147 led = 1;
tommyzhu19951204 0:5252dc3a058e 148 else
tommyzhu19951204 0:5252dc3a058e 149 led = 0;
tommyzhu19951204 0:5252dc3a058e 150 pc.printf("%5.1f,%4.2f,%d,%3.1f\n", print_angle, print_rev, i, target);
tommyzhu19951204 0:5252dc3a058e 151 }
tommyzhu19951204 0:5252dc3a058e 152 }
tommyzhu19951204 0:5252dc3a058e 153 }