EE192 Team 4 / Mbed 2 deprecated Everything

Everything put together

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

main.cpp@0:fcf070a88ba0, 2016-03-18 (annotated)

Committer:
vsutardja
Date:
Fri Mar 18 22:33:47 2016 +0000
Revision:
0:fcf070a88ba0
Child:
1:32610c005260
Figure 8 checkpoint working

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