EE192 Team 4 / Mbed 2 deprecated FixedPWM

Fixed PWM

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

Fork of Sequential_Timing by EE192 Team 4

main.cpp@1:32610c005260, 2016-03-29 (annotated)

Committer:
vsutardja
Date:
Tue Mar 29 22:00:52 2016 +0000
Revision:
1:32610c005260
Parent:
0:fcf070a88ba0
Child:
2:a8adff46eaca
Controller

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 1:32610c005260 49 float Kp = 13.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 cmd = bt.getc();
vsutardja 0:fcf070a88ba0 96 while (cmd != 'q') {
vsutardja 0:fcf070a88ba0 97 switch(atoi(&cmd)){
vsutardja 0:fcf070a88ba0 98 case 0:
vsutardja 0:fcf070a88ba0 99 bt.printf("Duty cycle: %f, Pulse count: %d, Velocity: %f, Kp: %f\r\n", d, curr_pulses, velocity, Kp);
vsutardja 0:fcf070a88ba0 100 break;
vsutardja 0:fcf070a88ba0 101 case 1:
vsutardja 0:fcf070a88ba0 102 bt.printf("Servo angle: %f, Track center: %d, t_int: %d\r\n", a, center, t_int);
vsutardja 0:fcf070a88ba0 103 break;
vsutardja 0:fcf070a88ba0 104 }
vsutardja 0:fcf070a88ba0 105 if (bt.readable()) {
vsutardja 0:fcf070a88ba0 106 cmd = bt.getc();
vsutardja 0:fcf070a88ba0 107 }
vsutardja 0:fcf070a88ba0 108 }
vsutardja 0:fcf070a88ba0 109 }
vsutardja 0:fcf070a88ba0 110 }
vsutardja 0:fcf070a88ba0 111
vsutardja 0:fcf070a88ba0 112 // Read data from camera
vsutardja 0:fcf070a88ba0 113 void read_camera() {
vsutardja 0:fcf070a88ba0 114 // Start data transfer
vsutardja 0:fcf070a88ba0 115 si = 1;
vsutardja 0:fcf070a88ba0 116 wait_us(1);
vsutardja 0:fcf070a88ba0 117 clk = 1;
vsutardja 0:fcf070a88ba0 118 wait_us(1);
vsutardja 0:fcf070a88ba0 119 si = 0;
vsutardja 0:fcf070a88ba0 120 wait_us(1);
vsutardja 0:fcf070a88ba0 121
vsutardja 0:fcf070a88ba0 122 // Read camera data
vsutardja 0:fcf070a88ba0 123 for (int i = 0; i < 128; i++) {
vsutardja 0:fcf070a88ba0 124 clk = 0;
vsutardja 0:fcf070a88ba0 125 img[i] = ao.read_u16();
vsutardja 0:fcf070a88ba0 126 clk = 1;
vsutardja 0:fcf070a88ba0 127 wait_us(1);
vsutardja 0:fcf070a88ba0 128 }
vsutardja 0:fcf070a88ba0 129 clk = 0;
vsutardja 0:fcf070a88ba0 130
vsutardja 0:fcf070a88ba0 131 // Update servo angle
vsutardja 0:fcf070a88ba0 132 track();
vsutardja 0:fcf070a88ba0 133
vsutardja 0:fcf070a88ba0 134 // Set next frame exposure time
vsutardja 0:fcf070a88ba0 135 camera_read.attach_us(&read_camera, t_int);
vsutardja 0:fcf070a88ba0 136 }
vsutardja 0:fcf070a88ba0 137
vsutardja 0:fcf070a88ba0 138 // Find line center from image
vsutardja 0:fcf070a88ba0 139 // Take two-point moving average to smooth the data
vsutardja 0:fcf070a88ba0 140 // Find indices where max and min of smoothed data are attained
vsutardja 0:fcf070a88ba0 141 // Calculate and return midpoint of argmax and argmin
vsutardja 0:fcf070a88ba0 142 void track() {
vsutardja 0:fcf070a88ba0 143 max = -1;
vsutardja 0:fcf070a88ba0 144 argmax = 0;
vsutardja 0:fcf070a88ba0 145 argmin = 0;
vsutardja 0:fcf070a88ba0 146 for (int i = 0; i < 107; i++) {
vsutardja 0:fcf070a88ba0 147 if (img[i+11] > max) {
vsutardja 0:fcf070a88ba0 148 max = img[i+11];
vsutardja 0:fcf070a88ba0 149 }
vsutardja 0:fcf070a88ba0 150 if (i == 126) {
vsutardja 0:fcf070a88ba0 151 temp[i-1] = (img[i+11] + img[i+1+11]) / 2 - temp[i-1];
vsutardja 0:fcf070a88ba0 152 if (temp[i-1] > temp[argmax]) {
vsutardja 0:fcf070a88ba0 153 argmax = i - 1;
vsutardja 0:fcf070a88ba0 154 }
vsutardja 0:fcf070a88ba0 155 if (temp[i-1] < temp[argmin]) {
vsutardja 0:fcf070a88ba0 156 argmin = i - 1;
vsutardja 0:fcf070a88ba0 157 }
vsutardja 0:fcf070a88ba0 158 } else {
vsutardja 0:fcf070a88ba0 159 temp[i] = (img[i+11] + img[i+1+11]) / 2;
vsutardja 0:fcf070a88ba0 160 if (i > 0) {
vsutardja 0:fcf070a88ba0 161 temp[i-1] = temp[i] - temp[i-1];
vsutardja 0:fcf070a88ba0 162 if (temp[i-1] > temp[argmax]) {
vsutardja 0:fcf070a88ba0 163 argmax = i - 1;
vsutardja 0:fcf070a88ba0 164 }
vsutardja 0:fcf070a88ba0 165 if (temp[i-1] < temp[argmin]) {
vsutardja 0:fcf070a88ba0 166 argmin = i - 1;
vsutardja 0:fcf070a88ba0 167 }
vsutardja 0:fcf070a88ba0 168 }
vsutardja 0:fcf070a88ba0 169 }
vsutardja 0:fcf070a88ba0 170 }
vsutardja 0:fcf070a88ba0 171
vsutardja 0:fcf070a88ba0 172 if (max > 43253) {
vsutardja 0:fcf070a88ba0 173 center = (argmax + argmin + 2 + 11) / 2;
vsutardja 0:fcf070a88ba0 174 a = 88 + (64 - center) * 0.625;
vsutardja 0:fcf070a88ba0 175 servo = a / 180;
vsutardja 0:fcf070a88ba0 176 }
vsutardja 0:fcf070a88ba0 177 }
vsutardja 0:fcf070a88ba0 178
vsutardja 0:fcf070a88ba0 179 // ====
vsutardja 0:fcf070a88ba0 180 // Main
vsutardja 0:fcf070a88ba0 181 // ====
vsutardja 0:fcf070a88ba0 182 int main() {
vsutardja 0:fcf070a88ba0 183 // Initialize motor
vsutardja 0:fcf070a88ba0 184 motor.period_us(T);
vsutardja 1:32610c005260 185 motor = 1.0;
vsutardja 1:32610c005260 186 wait(7);
vsutardja 0:fcf070a88ba0 187 motor = 1.0 - d;
vsutardja 0:fcf070a88ba0 188
vsutardja 0:fcf070a88ba0 189 // Initialize servo
vsutardja 0:fcf070a88ba0 190 servo.calibrate(0.001, 45.0);
vsutardja 0:fcf070a88ba0 191 servo = a / 180.0;
vsutardja 0:fcf070a88ba0 192
vsutardja 0:fcf070a88ba0 193 // Initialize & start camera
vsutardja 0:fcf070a88ba0 194 clk = 0;
vsutardja 1:32610c005260 195 // read_camera();
vsutardja 0:fcf070a88ba0 196
vsutardja 0:fcf070a88ba0 197 // Initialize motor controller
vsutardja 0:fcf070a88ba0 198 motor_ctrl.setInputLimits(0.0, 10.0);
vsutardja 0:fcf070a88ba0 199 motor_ctrl.setOutputLimits(0.1, 0.5);
vsutardja 1:32610c005260 200 motor_ctrl.setSetPoint(3);
vsutardja 0:fcf070a88ba0 201 motor_ctrl.setBias(0.0);
vsutardja 0:fcf070a88ba0 202 motor_ctrl.setMode(1);
vsutardja 0:fcf070a88ba0 203
vsutardja 0:fcf070a88ba0 204 // Initialize bluetooth
vsutardja 0:fcf070a88ba0 205 bt.baud(115200);
vsutardja 0:fcf070a88ba0 206
vsutardja 0:fcf070a88ba0 207 // Initialize communications thread
vsutardja 0:fcf070a88ba0 208 Thread communication_thread(communication);
vsutardja 0:fcf070a88ba0 209
vsutardja 0:fcf070a88ba0 210 // Start motor controller
vsutardja 1:32610c005260 211 // motor = 0.85;
vsutardja 1:32610c005260 212
vsutardja 1:32610c005260 213
vsutardja 0:fcf070a88ba0 214 while (true) {
vsutardja 0:fcf070a88ba0 215 curr_pulses = qei.getPulses();
vsutardja 0:fcf070a88ba0 216 velocity = (curr_pulses - prev_pulses)/ interval / ppr * c;
vsutardja 0:fcf070a88ba0 217 prev_pulses = curr_pulses;
vsutardja 0:fcf070a88ba0 218 motor_ctrl.setProcessValue(velocity);
vsutardja 0:fcf070a88ba0 219 d = motor_ctrl.compute();
vsutardja 0:fcf070a88ba0 220 motor = 1.0 - d;
vsutardja 0:fcf070a88ba0 221 wait(interval);
vsutardja 0:fcf070a88ba0 222 }
vsutardja 0:fcf070a88ba0 223 }