EE192 Team 4 / Mbed 2 deprecated Everything

Everything put together

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

main.cpp@5:7cba3ffc38bb, 2016-04-01 (annotated)

Committer:
vsutardja
Date:
Fri Apr 01 01:26:59 2016 +0000
Revision:
5:7cba3ffc38bb
Parent:
4:947c3634b649
Child:
6:f7a2197dd8aa
Telemetry

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