![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Fixed PWM
Dependencies: FastAnalogIn MODSERIAL PID QEI RPCInterface Servo mbed-rtos mbed telemetry
Fork of Sequential_Timing by
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?
User | Revision | Line number | New 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 | } |