EE192 Team 4 / Mbed 2 deprecated Sequential_Timing

One big fixed period control loop

Dependencies:   FastAnalogIn MODSERIAL PID QEI RPCInterface Servo mbed-rtos mbed telemetry PinDetect

Fork of Everything by EE192 Team 4

main.cpp@2:a8adff46eaca, 2016-03-29 (annotated)

Committer:
vsutardja
Date:
Tue Mar 29 23:31:32 2016 +0000
Revision:
2:a8adff46eaca
Parent:
1:32610c005260
Child:
3:c57674c348bd
Velocity moving weighted average

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsutardja 0:fcf070a88ba0 1 #include "mbed.h"
vsutardja 0:fcf070a88ba0 2 #include "rtos.h"
vsutardja 0:fcf070a88ba0 3 #include "Servo.h"
vsutardja 0:fcf070a88ba0 4 #include "FastAnalogIn.h"
vsutardja 0:fcf070a88ba0 5 #include "PID.h"
vsutardja 0:fcf070a88ba0 6 #include "QEI.h"
vsutardja 0:fcf070a88ba0 7 #include "telemetry.h"
vsutardja 0:fcf070a88ba0 8
vsutardja 0:fcf070a88ba0 9 // =========
vsutardja 0:fcf070a88ba0 10 // Telemetry
vsutardja 0:fcf070a88ba0 11 // =========
vsutardja 0:fcf070a88ba0 12 //MODSERIAL telemetry_serial(PTC4, PTC3); // TX, RX
vsutardja 0:fcf070a88ba0 13 //telemetry::MbedHal telemetry_hal(telemetry_serial); // Hardware Abstraction Layer
vsutardja 0:fcf070a88ba0 14 //telemetry::Telemetry telemetry_obj(telemetry_hal); // Telemetry
vsutardja 0:fcf070a88ba0 15
vsutardja 0:fcf070a88ba0 16
vsutardja 0:fcf070a88ba0 17 // =============
vsutardja 0:fcf070a88ba0 18 // Communication
vsutardja 0:fcf070a88ba0 19 // =============
vsutardja 0:fcf070a88ba0 20 Serial pc(USBTX, USBRX); // USB connection
vsutardja 0:fcf070a88ba0 21 Serial bt(PTC4, PTC3); // BlueSMiRF connection
vsutardja 0:fcf070a88ba0 22 //int idx = 0;
vsutardja 0:fcf070a88ba0 23 char cmd; // Command
vsutardja 0:fcf070a88ba0 24 char ch;
vsutardja 0:fcf070a88ba0 25 char in[2];
vsutardja 0:fcf070a88ba0 26
vsutardja 0:fcf070a88ba0 27 void communication(void const *args); // Communications
vsutardja 0:fcf070a88ba0 28
vsutardja 0:fcf070a88ba0 29 // =====
vsutardja 0:fcf070a88ba0 30 // Motor
vsutardja 0:fcf070a88ba0 31 // =====
vsutardja 0:fcf070a88ba0 32 PwmOut motor(PTA4); // Enable pin (PWM)
vsutardja 0:fcf070a88ba0 33 int T = 25000; // Frequency
vsutardja 0:fcf070a88ba0 34 float d = 0.0; // Duty cycle
vsutardja 0:fcf070a88ba0 35
vsutardja 0:fcf070a88ba0 36 // =======
vsutardja 0:fcf070a88ba0 37 // Encoder
vsutardja 0:fcf070a88ba0 38 // =======
vsutardja 2:a8adff46eaca 39 const int MVG_AVG = 4;
vsutardja 0:fcf070a88ba0 40 int ppr = 389; // Pulses per revolution
vsutardja 0:fcf070a88ba0 41 QEI qei(PTD3, PTD2, NC, ppr, QEI::X4_ENCODING); // Quadrature encoder
vsutardja 0:fcf070a88ba0 42 float c = 0.20106; // Wheel circumference
vsutardja 0:fcf070a88ba0 43 int prev_pulses = 0; // Previous pulse count
vsutardja 0:fcf070a88ba0 44 int curr_pulses = 0; // Current pulse count
vsutardja 0:fcf070a88ba0 45 float velocity = 0; // Velocity
vsutardja 2:a8adff46eaca 46 float v_prev[MVG_AVG] = {0};
vsutardja 0:fcf070a88ba0 47
vsutardja 0:fcf070a88ba0 48 // ========
vsutardja 0:fcf070a88ba0 49 // Velocity
vsutardja 0:fcf070a88ba0 50 // ========
vsutardja 1:32610c005260 51 float Kp = 13.0; // Proportional factor
vsutardja 0:fcf070a88ba0 52 float Ki = 0; // Integral factor
vsutardja 0:fcf070a88ba0 53 float Kd = 0; // Derivative factor
vsutardja 0:fcf070a88ba0 54 float interval = 0.01; // Sampling interval
vsutardja 0:fcf070a88ba0 55 PID motor_ctrl(Kp, Ki, Kd, interval); // Motor controller
vsutardja 0:fcf070a88ba0 56
vsutardja 0:fcf070a88ba0 57 // =====
vsutardja 0:fcf070a88ba0 58 // Servo
vsutardja 0:fcf070a88ba0 59 // =====
vsutardja 0:fcf070a88ba0 60 Servo servo(PTA12); // Enable pin (PWM)
vsutardja 0:fcf070a88ba0 61 float a = 88; // Angle
vsutardja 0:fcf070a88ba0 62
vsutardja 0:fcf070a88ba0 63 // ======
vsutardja 0:fcf070a88ba0 64 // Camera
vsutardja 0:fcf070a88ba0 65 // ======
vsutardja 0:fcf070a88ba0 66 DigitalOut clk(PTD5); // Clock pin
vsutardja 0:fcf070a88ba0 67 DigitalOut si(PTD0); // SI pin
vsutardja 0:fcf070a88ba0 68 FastAnalogIn ao(PTC2); // AO pin
vsutardja 0:fcf070a88ba0 69 Timeout camera_read; // Camera read timeout
vsutardja 0:fcf070a88ba0 70 int t_int = 15000; // Exposure time
vsutardja 0:fcf070a88ba0 71 int img[128]; // Image data
vsutardja 0:fcf070a88ba0 72
vsutardja 0:fcf070a88ba0 73 void readCamera(); // Read data from camera
vsutardja 0:fcf070a88ba0 74
vsutardja 0:fcf070a88ba0 75 // ================
vsutardja 0:fcf070a88ba0 76 // Image processing
vsutardja 0:fcf070a88ba0 77 // ================
vsutardja 0:fcf070a88ba0 78 int max = -1;
vsutardja 0:fcf070a88ba0 79 int argmax = 0;
vsutardja 0:fcf070a88ba0 80 int argmin = 0;
vsutardja 0:fcf070a88ba0 81 int temp[128];
vsutardja 0:fcf070a88ba0 82 int center;
vsutardja 0:fcf070a88ba0 83
vsutardja 0:fcf070a88ba0 84 void track(); // Line-tracking steering
vsutardja 0:fcf070a88ba0 85
vsutardja 0:fcf070a88ba0 86 // ================
vsutardja 0:fcf070a88ba0 87 // Functions
vsutardja 0:fcf070a88ba0 88 // ================
vsutardja 0:fcf070a88ba0 89
vsutardja 0:fcf070a88ba0 90 // Communications
vsutardja 0:fcf070a88ba0 91 void communication(void const *args) {
vsutardja 0:fcf070a88ba0 92 while (true) {
vsutardja 0:fcf070a88ba0 93 bt.printf("\r\nPress q to return to this prompt.\r\n");
vsutardja 0:fcf070a88ba0 94 bt.printf("Available diagnostics:\r\n");
vsutardja 0:fcf070a88ba0 95 bt.printf(" [0] Velocity\r\n");
vsutardja 0:fcf070a88ba0 96 bt.printf(" [1] Steering\r\n");
vsutardja 2:a8adff46eaca 97 bt.printf(" [2] Change Kp\r\n");
vsutardja 2:a8adff46eaca 98 bt.printf(" [3] Change exposure time\r\n");
vsutardja 0:fcf070a88ba0 99 cmd = bt.getc();
vsutardja 0:fcf070a88ba0 100 while (cmd != 'q') {
vsutardja 0:fcf070a88ba0 101 switch(atoi(&cmd)){
vsutardja 0:fcf070a88ba0 102 case 0:
vsutardja 0:fcf070a88ba0 103 bt.printf("Duty cycle: %f, Pulse count: %d, Velocity: %f, Kp: %f\r\n", d, curr_pulses, velocity, Kp);
vsutardja 0:fcf070a88ba0 104 break;
vsutardja 0:fcf070a88ba0 105 case 1:
vsutardja 0:fcf070a88ba0 106 bt.printf("Servo angle: %f, Track center: %d, t_int: %d\r\n", a, center, t_int);
vsutardja 0:fcf070a88ba0 107 break;
vsutardja 2:a8adff46eaca 108 case 2:
vsutardja 2:a8adff46eaca 109 // idx = 0;
vsutardja 2:a8adff46eaca 110 bt.printf("Current Kp = %f. Enter new Kp: ", Kp);
vsutardja 2:a8adff46eaca 111 // while ((cmd = bt.getc()) != '\n') {
vsutardja 2:a8adff46eaca 112 for (int idx = 0; idx < 2; idx++) {
vsutardja 2:a8adff46eaca 113 ch = bt.getc();
vsutardja 2:a8adff46eaca 114 in[idx] = ch;
vsutardja 2:a8adff46eaca 115 // idx = idx + 1;
vsutardja 2:a8adff46eaca 116 }
vsutardja 2:a8adff46eaca 117 bt.printf("\r\n");
vsutardja 2:a8adff46eaca 118 Kp = atof(in);
vsutardja 2:a8adff46eaca 119 break;
vsutardja 2:a8adff46eaca 120 case 3:
vsutardja 2:a8adff46eaca 121 // idx = 0;
vsutardja 2:a8adff46eaca 122 bt.printf("Current t_int = %dms. Enter new t_int (ms): ", t_int / 1000);
vsutardja 2:a8adff46eaca 123 // while ((cmd = bt.getc()) != '\n') {
vsutardja 2:a8adff46eaca 124 for (int idx = 0; idx < 2; idx++) {
vsutardja 2:a8adff46eaca 125 ch = bt.getc();
vsutardja 2:a8adff46eaca 126 in[idx] = ch;
vsutardja 2:a8adff46eaca 127 // idx = idx + 1;
vsutardja 2:a8adff46eaca 128 }
vsutardja 2:a8adff46eaca 129 bt.printf("\r\n");
vsutardja 2:a8adff46eaca 130 t_int = atoi(in);
vsutardja 2:a8adff46eaca 131 break;
vsutardja 0:fcf070a88ba0 132 }
vsutardja 0:fcf070a88ba0 133 if (bt.readable()) {
vsutardja 0:fcf070a88ba0 134 cmd = bt.getc();
vsutardja 0:fcf070a88ba0 135 }
vsutardja 0:fcf070a88ba0 136 }
vsutardja 0:fcf070a88ba0 137 }
vsutardja 0:fcf070a88ba0 138 }
vsutardja 0:fcf070a88ba0 139
vsutardja 0:fcf070a88ba0 140 // Read data from camera
vsutardja 0:fcf070a88ba0 141 void read_camera() {
vsutardja 0:fcf070a88ba0 142 // Start data transfer
vsutardja 0:fcf070a88ba0 143 si = 1;
vsutardja 0:fcf070a88ba0 144 wait_us(1);
vsutardja 0:fcf070a88ba0 145 clk = 1;
vsutardja 0:fcf070a88ba0 146 wait_us(1);
vsutardja 0:fcf070a88ba0 147 si = 0;
vsutardja 0:fcf070a88ba0 148 wait_us(1);
vsutardja 0:fcf070a88ba0 149
vsutardja 0:fcf070a88ba0 150 // Read camera data
vsutardja 0:fcf070a88ba0 151 for (int i = 0; i < 128; i++) {
vsutardja 0:fcf070a88ba0 152 clk = 0;
vsutardja 0:fcf070a88ba0 153 img[i] = ao.read_u16();
vsutardja 0:fcf070a88ba0 154 clk = 1;
vsutardja 0:fcf070a88ba0 155 wait_us(1);
vsutardja 0:fcf070a88ba0 156 }
vsutardja 0:fcf070a88ba0 157 clk = 0;
vsutardja 0:fcf070a88ba0 158
vsutardja 0:fcf070a88ba0 159 // Update servo angle
vsutardja 0:fcf070a88ba0 160 track();
vsutardja 0:fcf070a88ba0 161
vsutardja 0:fcf070a88ba0 162 // Set next frame exposure time
vsutardja 0:fcf070a88ba0 163 camera_read.attach_us(&read_camera, t_int);
vsutardja 0:fcf070a88ba0 164 }
vsutardja 0:fcf070a88ba0 165
vsutardja 0:fcf070a88ba0 166 // Find line center from image
vsutardja 0:fcf070a88ba0 167 // Take two-point moving average to smooth the data
vsutardja 0:fcf070a88ba0 168 // Find indices where max and min of smoothed data are attained
vsutardja 0:fcf070a88ba0 169 // Calculate and return midpoint of argmax and argmin
vsutardja 0:fcf070a88ba0 170 void track() {
vsutardja 0:fcf070a88ba0 171 max = -1;
vsutardja 0:fcf070a88ba0 172 argmax = 0;
vsutardja 0:fcf070a88ba0 173 argmin = 0;
vsutardja 0:fcf070a88ba0 174 for (int i = 0; i < 107; i++) {
vsutardja 0:fcf070a88ba0 175 if (img[i+11] > max) {
vsutardja 0:fcf070a88ba0 176 max = img[i+11];
vsutardja 0:fcf070a88ba0 177 }
vsutardja 0:fcf070a88ba0 178 if (i == 126) {
vsutardja 0:fcf070a88ba0 179 temp[i-1] = (img[i+11] + img[i+1+11]) / 2 - temp[i-1];
vsutardja 0:fcf070a88ba0 180 if (temp[i-1] > temp[argmax]) {
vsutardja 0:fcf070a88ba0 181 argmax = i - 1;
vsutardja 0:fcf070a88ba0 182 }
vsutardja 0:fcf070a88ba0 183 if (temp[i-1] < temp[argmin]) {
vsutardja 0:fcf070a88ba0 184 argmin = i - 1;
vsutardja 0:fcf070a88ba0 185 }
vsutardja 0:fcf070a88ba0 186 } else {
vsutardja 0:fcf070a88ba0 187 temp[i] = (img[i+11] + img[i+1+11]) / 2;
vsutardja 0:fcf070a88ba0 188 if (i > 0) {
vsutardja 0:fcf070a88ba0 189 temp[i-1] = temp[i] - temp[i-1];
vsutardja 0:fcf070a88ba0 190 if (temp[i-1] > temp[argmax]) {
vsutardja 0:fcf070a88ba0 191 argmax = i - 1;
vsutardja 0:fcf070a88ba0 192 }
vsutardja 0:fcf070a88ba0 193 if (temp[i-1] < temp[argmin]) {
vsutardja 0:fcf070a88ba0 194 argmin = i - 1;
vsutardja 0:fcf070a88ba0 195 }
vsutardja 0:fcf070a88ba0 196 }
vsutardja 0:fcf070a88ba0 197 }
vsutardja 0:fcf070a88ba0 198 }
vsutardja 0:fcf070a88ba0 199
vsutardja 0:fcf070a88ba0 200 if (max > 43253) {
vsutardja 0:fcf070a88ba0 201 center = (argmax + argmin + 2 + 11) / 2;
vsutardja 0:fcf070a88ba0 202 a = 88 + (64 - center) * 0.625;
vsutardja 0:fcf070a88ba0 203 servo = a / 180;
vsutardja 0:fcf070a88ba0 204 }
vsutardja 0:fcf070a88ba0 205 }
vsutardja 0:fcf070a88ba0 206
vsutardja 0:fcf070a88ba0 207 // ====
vsutardja 0:fcf070a88ba0 208 // Main
vsutardja 0:fcf070a88ba0 209 // ====
vsutardja 0:fcf070a88ba0 210 int main() {
vsutardja 2:a8adff46eaca 211
vsutardja 0:fcf070a88ba0 212 // Initialize motor
vsutardja 0:fcf070a88ba0 213 motor.period_us(T);
vsutardja 0:fcf070a88ba0 214 motor = 1.0 - d;
vsutardja 0:fcf070a88ba0 215
vsutardja 0:fcf070a88ba0 216 // Initialize servo
vsutardja 0:fcf070a88ba0 217 servo.calibrate(0.001, 45.0);
vsutardja 0:fcf070a88ba0 218 servo = a / 180.0;
vsutardja 0:fcf070a88ba0 219
vsutardja 0:fcf070a88ba0 220 // Initialize & start camera
vsutardja 0:fcf070a88ba0 221 clk = 0;
vsutardja 1:32610c005260 222 // read_camera();
vsutardja 0:fcf070a88ba0 223
vsutardja 0:fcf070a88ba0 224 // Initialize motor controller
vsutardja 0:fcf070a88ba0 225 motor_ctrl.setInputLimits(0.0, 10.0);
vsutardja 0:fcf070a88ba0 226 motor_ctrl.setOutputLimits(0.1, 0.5);
vsutardja 2:a8adff46eaca 227 motor_ctrl.setSetPoint(1.5);
vsutardja 0:fcf070a88ba0 228 motor_ctrl.setBias(0.0);
vsutardja 0:fcf070a88ba0 229 motor_ctrl.setMode(1);
vsutardja 0:fcf070a88ba0 230
vsutardja 0:fcf070a88ba0 231 // Initialize bluetooth
vsutardja 0:fcf070a88ba0 232 bt.baud(115200);
vsutardja 0:fcf070a88ba0 233
vsutardja 0:fcf070a88ba0 234 // Initialize communications thread
vsutardja 0:fcf070a88ba0 235 Thread communication_thread(communication);
vsutardja 0:fcf070a88ba0 236
vsutardja 0:fcf070a88ba0 237 // Start motor controller
vsutardja 0:fcf070a88ba0 238 while (true) {
vsutardja 0:fcf070a88ba0 239 curr_pulses = qei.getPulses();
vsutardja 2:a8adff46eaca 240 for (int i = 0; i < MVG_AVG - 1; i++) {
vsutardja 2:a8adff46eaca 241 v_prev[i] = abs(1.5 - 0.5 * (1.5 - v_prev[i+1]));
vsutardja 2:a8adff46eaca 242 }
vsutardja 2:a8adff46eaca 243 v_prev[MVG_AVG-1] = velocity;
vsutardja 0:fcf070a88ba0 244 velocity = (curr_pulses - prev_pulses)/ interval / ppr * c;
vsutardja 2:a8adff46eaca 245 for (int i = 0; i < MVG_AVG; i++) {
vsutardja 2:a8adff46eaca 246 velocity = velocity + v_prev[i];
vsutardja 2:a8adff46eaca 247 }
vsutardja 2:a8adff46eaca 248 velocity = velocity / (MVG_AVG + 1.0);
vsutardja 0:fcf070a88ba0 249 prev_pulses = curr_pulses;
vsutardja 0:fcf070a88ba0 250 motor_ctrl.setProcessValue(velocity);
vsutardja 0:fcf070a88ba0 251 d = motor_ctrl.compute();
vsutardja 0:fcf070a88ba0 252 motor = 1.0 - d;
vsutardja 0:fcf070a88ba0 253 wait(interval);
vsutardja 2:a8adff46eaca 254 pc.printf("Velocity: %f\r\n", velocity);
vsutardja 0:fcf070a88ba0 255 }
vsutardja 0:fcf070a88ba0 256 }