EE192 Team 4 / Mbed 2 deprecated Everything

Everything put together

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

main.cpp@3:c57674c348bd, 2016-03-30 (annotated)

Committer:
vsutardja
Date:
Wed Mar 30 01:21:39 2016 +0000
Revision:
3:c57674c348bd
Parent:
2:a8adff46eaca
Child:
4:947c3634b649
Contrast based exposure AGC

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 3:c57674c348bd 79 float lum_bg = 0;
vsutardja 3:c57674c348bd 80 float contrast = 0;
vsutardja 0:fcf070a88ba0 81 int argmax = 0;
vsutardja 0:fcf070a88ba0 82 int argmin = 0;
vsutardja 0:fcf070a88ba0 83 int temp[128];
vsutardja 0:fcf070a88ba0 84 int center;
vsutardja 0:fcf070a88ba0 85
vsutardja 0:fcf070a88ba0 86 void track(); // Line-tracking steering
vsutardja 0:fcf070a88ba0 87
vsutardja 0:fcf070a88ba0 88 // ================
vsutardja 0:fcf070a88ba0 89 // Functions
vsutardja 0:fcf070a88ba0 90 // ================
vsutardja 0:fcf070a88ba0 91
vsutardja 0:fcf070a88ba0 92 // Communications
vsutardja 0:fcf070a88ba0 93 void communication(void const *args) {
vsutardja 0:fcf070a88ba0 94 while (true) {
vsutardja 0:fcf070a88ba0 95 bt.printf("\r\nPress q to return to this prompt.\r\n");
vsutardja 0:fcf070a88ba0 96 bt.printf("Available diagnostics:\r\n");
vsutardja 0:fcf070a88ba0 97 bt.printf(" [0] Velocity\r\n");
vsutardja 0:fcf070a88ba0 98 bt.printf(" [1] Steering\r\n");
vsutardja 2:a8adff46eaca 99 bt.printf(" [2] Change Kp\r\n");
vsutardja 2:a8adff46eaca 100 bt.printf(" [3] Change exposure time\r\n");
vsutardja 0:fcf070a88ba0 101 cmd = bt.getc();
vsutardja 0:fcf070a88ba0 102 while (cmd != 'q') {
vsutardja 0:fcf070a88ba0 103 switch(atoi(&cmd)){
vsutardja 0:fcf070a88ba0 104 case 0:
vsutardja 0:fcf070a88ba0 105 bt.printf("Duty cycle: %f, Pulse count: %d, Velocity: %f, Kp: %f\r\n", d, curr_pulses, velocity, Kp);
vsutardja 0:fcf070a88ba0 106 break;
vsutardja 0:fcf070a88ba0 107 case 1:
vsutardja 0:fcf070a88ba0 108 bt.printf("Servo angle: %f, Track center: %d, t_int: %d\r\n", a, center, t_int);
vsutardja 0:fcf070a88ba0 109 break;
vsutardja 2:a8adff46eaca 110 case 2:
vsutardja 2:a8adff46eaca 111 // idx = 0;
vsutardja 2:a8adff46eaca 112 bt.printf("Current Kp = %f. Enter new Kp: ", Kp);
vsutardja 2:a8adff46eaca 113 // while ((cmd = bt.getc()) != '\n') {
vsutardja 2:a8adff46eaca 114 for (int idx = 0; idx < 2; idx++) {
vsutardja 2:a8adff46eaca 115 ch = bt.getc();
vsutardja 2:a8adff46eaca 116 in[idx] = ch;
vsutardja 2:a8adff46eaca 117 // idx = idx + 1;
vsutardja 2:a8adff46eaca 118 }
vsutardja 2:a8adff46eaca 119 bt.printf("\r\n");
vsutardja 2:a8adff46eaca 120 Kp = atof(in);
vsutardja 2:a8adff46eaca 121 break;
vsutardja 2:a8adff46eaca 122 case 3:
vsutardja 2:a8adff46eaca 123 // idx = 0;
vsutardja 2:a8adff46eaca 124 bt.printf("Current t_int = %dms. Enter new t_int (ms): ", t_int / 1000);
vsutardja 2:a8adff46eaca 125 // while ((cmd = bt.getc()) != '\n') {
vsutardja 2:a8adff46eaca 126 for (int idx = 0; idx < 2; idx++) {
vsutardja 2:a8adff46eaca 127 ch = bt.getc();
vsutardja 2:a8adff46eaca 128 in[idx] = ch;
vsutardja 2:a8adff46eaca 129 // idx = idx + 1;
vsutardja 2:a8adff46eaca 130 }
vsutardja 2:a8adff46eaca 131 bt.printf("\r\n");
vsutardja 2:a8adff46eaca 132 t_int = atoi(in);
vsutardja 2:a8adff46eaca 133 break;
vsutardja 0:fcf070a88ba0 134 }
vsutardja 0:fcf070a88ba0 135 if (bt.readable()) {
vsutardja 0:fcf070a88ba0 136 cmd = bt.getc();
vsutardja 0:fcf070a88ba0 137 }
vsutardja 0:fcf070a88ba0 138 }
vsutardja 0:fcf070a88ba0 139 }
vsutardja 0:fcf070a88ba0 140 }
vsutardja 0:fcf070a88ba0 141
vsutardja 0:fcf070a88ba0 142 // Read data from camera
vsutardja 0:fcf070a88ba0 143 void read_camera() {
vsutardja 0:fcf070a88ba0 144 // Start data transfer
vsutardja 0:fcf070a88ba0 145 si = 1;
vsutardja 0:fcf070a88ba0 146 wait_us(1);
vsutardja 0:fcf070a88ba0 147 clk = 1;
vsutardja 0:fcf070a88ba0 148 wait_us(1);
vsutardja 0:fcf070a88ba0 149 si = 0;
vsutardja 0:fcf070a88ba0 150 wait_us(1);
vsutardja 0:fcf070a88ba0 151
vsutardja 0:fcf070a88ba0 152 // Read camera data
vsutardja 0:fcf070a88ba0 153 for (int i = 0; i < 128; i++) {
vsutardja 0:fcf070a88ba0 154 clk = 0;
vsutardja 0:fcf070a88ba0 155 img[i] = ao.read_u16();
vsutardja 0:fcf070a88ba0 156 clk = 1;
vsutardja 0:fcf070a88ba0 157 wait_us(1);
vsutardja 0:fcf070a88ba0 158 }
vsutardja 0:fcf070a88ba0 159 clk = 0;
vsutardja 0:fcf070a88ba0 160
vsutardja 0:fcf070a88ba0 161 // Update servo angle
vsutardja 0:fcf070a88ba0 162 track();
vsutardja 0:fcf070a88ba0 163
vsutardja 0:fcf070a88ba0 164 // Set next frame exposure time
vsutardja 3:c57674c348bd 165 // camera_read.attach_us(&read_camera, t_int);
vsutardja 0:fcf070a88ba0 166 }
vsutardja 0:fcf070a88ba0 167
vsutardja 0:fcf070a88ba0 168 // Find line center from image
vsutardja 0:fcf070a88ba0 169 // Take two-point moving average to smooth the data
vsutardja 0:fcf070a88ba0 170 // Find indices where max and min of smoothed data are attained
vsutardja 0:fcf070a88ba0 171 // Calculate and return midpoint of argmax and argmin
vsutardja 0:fcf070a88ba0 172 void track() {
vsutardja 0:fcf070a88ba0 173 max = -1;
vsutardja 3:c57674c348bd 174 lum_bg = 0;
vsutardja 0:fcf070a88ba0 175 argmax = 0;
vsutardja 0:fcf070a88ba0 176 argmin = 0;
vsutardja 0:fcf070a88ba0 177 for (int i = 0; i < 107; i++) {
vsutardja 0:fcf070a88ba0 178 if (img[i+11] > max) {
vsutardja 0:fcf070a88ba0 179 max = img[i+11];
vsutardja 0:fcf070a88ba0 180 }
vsutardja 0:fcf070a88ba0 181 if (i == 126) {
vsutardja 0:fcf070a88ba0 182 temp[i-1] = (img[i+11] + img[i+1+11]) / 2 - temp[i-1];
vsutardja 0:fcf070a88ba0 183 if (temp[i-1] > temp[argmax]) {
vsutardja 0:fcf070a88ba0 184 argmax = i - 1;
vsutardja 0:fcf070a88ba0 185 }
vsutardja 0:fcf070a88ba0 186 if (temp[i-1] < temp[argmin]) {
vsutardja 0:fcf070a88ba0 187 argmin = i - 1;
vsutardja 0:fcf070a88ba0 188 }
vsutardja 0:fcf070a88ba0 189 } else {
vsutardja 0:fcf070a88ba0 190 temp[i] = (img[i+11] + img[i+1+11]) / 2;
vsutardja 0:fcf070a88ba0 191 if (i > 0) {
vsutardja 0:fcf070a88ba0 192 temp[i-1] = temp[i] - temp[i-1];
vsutardja 0:fcf070a88ba0 193 if (temp[i-1] > temp[argmax]) {
vsutardja 0:fcf070a88ba0 194 argmax = i - 1;
vsutardja 0:fcf070a88ba0 195 }
vsutardja 0:fcf070a88ba0 196 if (temp[i-1] < temp[argmin]) {
vsutardja 0:fcf070a88ba0 197 argmin = i - 1;
vsutardja 0:fcf070a88ba0 198 }
vsutardja 0:fcf070a88ba0 199 }
vsutardja 0:fcf070a88ba0 200 }
vsutardja 0:fcf070a88ba0 201 }
vsutardja 0:fcf070a88ba0 202
vsutardja 3:c57674c348bd 203 for (int i = 0; i < 30; i++) {
vsutardja 3:c57674c348bd 204 lum_bg = lum_bg + img[64 - 10 - i] / 60.0 + img[64 + 10 + i] / 60.0;
vsutardja 3:c57674c348bd 205 }
vsutardja 3:c57674c348bd 206
vsutardja 3:c57674c348bd 207 contrast = (max - lum_bg) / lum_bg;
vsutardja 3:c57674c348bd 208
vsutardja 3:c57674c348bd 209 if (contrast < 1.5) {
vsutardja 3:c57674c348bd 210 // Underexposed
vsutardja 3:c57674c348bd 211 if (max < 55000) {
vsutardja 3:c57674c348bd 212 t_int = t_int + (55000 - max);
vsutardja 3:c57674c348bd 213 }
vsutardja 3:c57674c348bd 214 // Overexposed
vsutardja 3:c57674c348bd 215 if (lum_bg > 25000) {
vsutardja 3:c57674c348bd 216 t_int = t_int - (lum_bg - 25000);
vsutardja 3:c57674c348bd 217 }
vsutardja 3:c57674c348bd 218 }
vsutardja 3:c57674c348bd 219
vsutardja 0:fcf070a88ba0 220 if (max > 43253) {
vsutardja 0:fcf070a88ba0 221 center = (argmax + argmin + 2 + 11) / 2;
vsutardja 0:fcf070a88ba0 222 a = 88 + (64 - center) * 0.625;
vsutardja 0:fcf070a88ba0 223 servo = a / 180;
vsutardja 0:fcf070a88ba0 224 }
vsutardja 3:c57674c348bd 225
vsutardja 3:c57674c348bd 226 camera_read.attach_us(&read_camera, t_int);
vsutardja 0:fcf070a88ba0 227 }
vsutardja 0:fcf070a88ba0 228
vsutardja 0:fcf070a88ba0 229 // ====
vsutardja 0:fcf070a88ba0 230 // Main
vsutardja 0:fcf070a88ba0 231 // ====
vsutardja 0:fcf070a88ba0 232 int main() {
vsutardja 2:a8adff46eaca 233
vsutardja 0:fcf070a88ba0 234 // Initialize motor
vsutardja 0:fcf070a88ba0 235 motor.period_us(T);
vsutardja 0:fcf070a88ba0 236 motor = 1.0 - d;
vsutardja 0:fcf070a88ba0 237
vsutardja 0:fcf070a88ba0 238 // Initialize servo
vsutardja 0:fcf070a88ba0 239 servo.calibrate(0.001, 45.0);
vsutardja 0:fcf070a88ba0 240 servo = a / 180.0;
vsutardja 0:fcf070a88ba0 241
vsutardja 0:fcf070a88ba0 242 // Initialize & start camera
vsutardja 0:fcf070a88ba0 243 clk = 0;
vsutardja 3:c57674c348bd 244 read_camera();
vsutardja 0:fcf070a88ba0 245
vsutardja 0:fcf070a88ba0 246 // Initialize motor controller
vsutardja 0:fcf070a88ba0 247 motor_ctrl.setInputLimits(0.0, 10.0);
vsutardja 0:fcf070a88ba0 248 motor_ctrl.setOutputLimits(0.1, 0.5);
vsutardja 2:a8adff46eaca 249 motor_ctrl.setSetPoint(1.5);
vsutardja 0:fcf070a88ba0 250 motor_ctrl.setBias(0.0);
vsutardja 0:fcf070a88ba0 251 motor_ctrl.setMode(1);
vsutardja 0:fcf070a88ba0 252
vsutardja 0:fcf070a88ba0 253 // Initialize bluetooth
vsutardja 0:fcf070a88ba0 254 bt.baud(115200);
vsutardja 0:fcf070a88ba0 255
vsutardja 0:fcf070a88ba0 256 // Initialize communications thread
vsutardja 0:fcf070a88ba0 257 Thread communication_thread(communication);
vsutardja 0:fcf070a88ba0 258
vsutardja 0:fcf070a88ba0 259 // Start motor controller
vsutardja 0:fcf070a88ba0 260 while (true) {
vsutardja 0:fcf070a88ba0 261 curr_pulses = qei.getPulses();
vsutardja 2:a8adff46eaca 262 for (int i = 0; i < MVG_AVG - 1; i++) {
vsutardja 2:a8adff46eaca 263 v_prev[i] = abs(1.5 - 0.5 * (1.5 - v_prev[i+1]));
vsutardja 2:a8adff46eaca 264 }
vsutardja 2:a8adff46eaca 265 v_prev[MVG_AVG-1] = velocity;
vsutardja 0:fcf070a88ba0 266 velocity = (curr_pulses - prev_pulses)/ interval / ppr * c;
vsutardja 2:a8adff46eaca 267 for (int i = 0; i < MVG_AVG; i++) {
vsutardja 2:a8adff46eaca 268 velocity = velocity + v_prev[i];
vsutardja 2:a8adff46eaca 269 }
vsutardja 2:a8adff46eaca 270 velocity = velocity / (MVG_AVG + 1.0);
vsutardja 0:fcf070a88ba0 271 prev_pulses = curr_pulses;
vsutardja 0:fcf070a88ba0 272 motor_ctrl.setProcessValue(velocity);
vsutardja 0:fcf070a88ba0 273 d = motor_ctrl.compute();
vsutardja 0:fcf070a88ba0 274 motor = 1.0 - d;
vsutardja 0:fcf070a88ba0 275 wait(interval);
vsutardja 3:c57674c348bd 276 pc.printf("BG Luminosity: %f, Feature Luminosity: %d, Contrast: %f, Exposure: %d\r\n", lum_bg, max, contrast, t_int);
vsutardja 0:fcf070a88ba0 277 }
vsutardja 0:fcf070a88ba0 278 }