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@11:4348bba086a4, 2016-04-05 (annotated)

Committer:
vsutardja
Date:
Tue Apr 05 21:52:32 2016 +0000
Revision:
11:4348bba086a4
Parent:
10:716484b1ddb5
Child:
12:54e7d8ff3a74
Telemetry cont'd

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