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@9:10fcf3d0ec2d, 2016-04-01 (annotated)

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