EE192 Team 4 / Mbed 2 deprecated Sequential_Timing

One big fixed period control loop

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

Fork of Everything by EE192 Team 4

main.cpp@10:716484b1ddb5, 2016-04-05 (annotated)

Committer:
vsutardja
Date:
Tue Apr 05 18:30:41 2016 +0000
Revision:
10:716484b1ddb5
Parent:
9:10fcf3d0ec2d
Child:
11:4348bba086a4
latest

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