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@12:54e7d8ff3a74, 2016-04-08 (annotated)

Committer:
vsutardja
Date:
Fri Apr 08 21:40:35 2016 +0000
Revision:
12:54e7d8ff3a74
Parent:
11:4348bba086a4
Child:
13:d6e167698517
init

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsutardja 12:54e7d8ff3a74 1 #include "FastAnalogIn.h"
vsutardja 0:fcf070a88ba0 2 #include "mbed.h"
vsutardja 0:fcf070a88ba0 3 #include "PID.h"
vsutardja 0:fcf070a88ba0 4 #include "QEI.h"
vsutardja 12:54e7d8ff3a74 5 #include "rtos.h"
vsutardja 12:54e7d8ff3a74 6 #include "SerialRPCInterface.h"
vsutardja 12:54e7d8ff3a74 7 #include "Servo.h"
vsutardja 0:fcf070a88ba0 8 #include "telemetry.h"
vsutardja 0:fcf070a88ba0 9
vsutardja 12:54e7d8ff3a74 10
vsutardja 0:fcf070a88ba0 11 // =========
vsutardja 0:fcf070a88ba0 12 // Telemetry
vsutardja 0:fcf070a88ba0 13 // =========
vsutardja 12:54e7d8ff3a74 14 MODSERIAL telemetry_serial(PTC4, PTC3); // TX, RX
vsutardja 12:54e7d8ff3a74 15 //MODSERIAL telemetry_serial(USBTX, USBRX);
vsutardja 12:54e7d8ff3a74 16 telemetry::MbedHal telemetry_hal(telemetry_serial); // Hardware Abstraction Layer
vsutardja 12:54e7d8ff3a74 17 telemetry::Telemetry telemetry_obj(telemetry_hal); // Telemetry
vsutardja 11:4348bba086a4 18
vsutardja 11:4348bba086a4 19 telemetry::Numeric<uint32_t> tele_time_ms(telemetry_obj, "time", "Time", "ms", 0);
vsutardja 12:54e7d8ff3a74 20 telemetry::NumericArray<uint16_t, 108> tele_linescan(telemetry_obj, "linescan", "Linescan", "ADC", 0);
vsutardja 12:54e7d8ff3a74 21 telemetry::Numeric<uint32_t> tele_exposure(telemetry_obj, "exposure", "Exposure", "us", 0);
vsutardja 12:54e7d8ff3a74 22 telemetry::Numeric<float> tele_velocity(telemetry_obj, "tele_vel", "Velocity", "who knows", 0);
vsutardja 12:54e7d8ff3a74 23 telemetry::Numeric<float> tele_pwm(telemetry_obj, "pwm", "PWM", "", 0);
vsutardja 12:54e7d8ff3a74 24 telemetry::Numeric<uint8_t> tele_center(telemetry_obj, "tele_center", "Center", "px", 0);
vsutardja 0:fcf070a88ba0 25
vsutardja 12:54e7d8ff3a74 26 Timer t;
vsutardja 12:54e7d8ff3a74 27 Timer t_tele;
vsutardja 12:54e7d8ff3a74 28 Ticker control_interrupt;
vsutardja 12:54e7d8ff3a74 29 int t_cam = 0;
vsutardja 12:54e7d8ff3a74 30 int t_steer = 0;
vsutardja 12:54e7d8ff3a74 31 int t_vel = 0;
vsutardja 12:54e7d8ff3a74 32
vsutardja 12:54e7d8ff3a74 33 float interrupt_T = 0.025;
vsutardja 0:fcf070a88ba0 34
vsutardja 0:fcf070a88ba0 35 // =============
vsutardja 0:fcf070a88ba0 36 // Communication
vsutardja 0:fcf070a88ba0 37 // =============
vsutardja 12:54e7d8ff3a74 38 char comm_cmd; // Command
vsutardja 12:54e7d8ff3a74 39 char comm_in[5]; // Input
vsutardja 12:54e7d8ff3a74 40 //Serial pc(USBTX, USBRX); // USB connection
vsutardja 12:54e7d8ff3a74 41 //Serial bt(PTC4, PTC3); // BlueSMiRF connection
vsutardja 0:fcf070a88ba0 42
vsutardja 12:54e7d8ff3a74 43 void communication(void const *args); // Communications
vsutardja 0:fcf070a88ba0 44
vsutardja 0:fcf070a88ba0 45 // =====
vsutardja 0:fcf070a88ba0 46 // Motor
vsutardja 0:fcf070a88ba0 47 // =====
vsutardja 12:54e7d8ff3a74 48 const int motor_T = 25000; // Frequency
vsutardja 12:54e7d8ff3a74 49 float motor_duty = 0.0; // Duty cycle
vsutardja 12:54e7d8ff3a74 50 bool e_stop = false; // Emergency stop
vsutardja 12:54e7d8ff3a74 51 PwmOut motor(PTA4); // Enable pin (PWM)
vsutardja 0:fcf070a88ba0 52
vsutardja 0:fcf070a88ba0 53 // =======
vsutardja 0:fcf070a88ba0 54 // Encoder
vsutardja 0:fcf070a88ba0 55 // =======
vsutardja 12:54e7d8ff3a74 56 const int ppr = 389; // Pulses per revolution
vsutardja 12:54e7d8ff3a74 57 const float c = 0.20106; // Wheel circumference
vsutardja 12:54e7d8ff3a74 58 int prev_pulses = 0; // Previous pulse count
vsutardja 12:54e7d8ff3a74 59 int curr_pulses = 0; // Current pulse count
vsutardja 12:54e7d8ff3a74 60 float velocity = 0; // Velocity
vsutardja 12:54e7d8ff3a74 61 QEI qei(PTD3, PTD2, NC, ppr, QEI::X4_ENCODING); // Quadrature encoder
vsutardja 0:fcf070a88ba0 62
vsutardja 0:fcf070a88ba0 63 // ========
vsutardja 0:fcf070a88ba0 64 // Velocity
vsutardja 0:fcf070a88ba0 65 // ========
vsutardja 12:54e7d8ff3a74 66 float Kp = 6.0; // Proportional factor
vsutardja 12:54e7d8ff3a74 67 float Ki = 0; // Integral factor
vsutardja 12:54e7d8ff3a74 68 float Kd = 0; // Derivative factor
vsutardja 12:54e7d8ff3a74 69 float interval = 0.01; // Sampling interval
vsutardja 12:54e7d8ff3a74 70 float ref_v = 0.8; // Reference velocity
vsutardja 12:54e7d8ff3a74 71 PID motor_ctrl(Kp, Ki, Kd, interval); // Motor controller
vsutardja 0:fcf070a88ba0 72
vsutardja 0:fcf070a88ba0 73 // =====
vsutardja 0:fcf070a88ba0 74 // Servo
vsutardja 0:fcf070a88ba0 75 // =====
vsutardja 12:54e7d8ff3a74 76 float angle = 88; // Angle
vsutardja 12:54e7d8ff3a74 77 float Ks = 0.9; // Steering proportion
vsutardja 12:54e7d8ff3a74 78 Servo servo(PTA12); // Signal pin (PWM)
vsutardja 0:fcf070a88ba0 79
vsutardja 0:fcf070a88ba0 80 // ======
vsutardja 0:fcf070a88ba0 81 // Camera
vsutardja 0:fcf070a88ba0 82 // ======
vsutardja 12:54e7d8ff3a74 83 int t_int = 10000; // Exposure time
vsutardja 12:54e7d8ff3a74 84 const int T_INT_MAX = interrupt_T * 1000000 - 10000; // Maximum exposure time
vsutardja 12:54e7d8ff3a74 85 const int T_INT_MIN = 35; // Minimum exposure time
vsutardja 12:54e7d8ff3a74 86 int img[108] = {0}; // Image data
vsutardja 12:54e7d8ff3a74 87 DigitalOut clk(PTD5); // Clock pin
vsutardja 12:54e7d8ff3a74 88 DigitalOut si(PTD0); // SI pin
vsutardja 12:54e7d8ff3a74 89 FastAnalogIn ao(PTC2); // AO pin
vsutardja 0:fcf070a88ba0 90
vsutardja 0:fcf070a88ba0 91 // ================
vsutardja 0:fcf070a88ba0 92 // Image processing
vsutardja 0:fcf070a88ba0 93 // ================
vsutardja 12:54e7d8ff3a74 94 int max = -1; // Maximum luminosity
vsutardja 12:54e7d8ff3a74 95 int left[5] = {0}; // Left edge
vsutardja 12:54e7d8ff3a74 96 int right[5] = {0}; // Right edge
vsutardja 12:54e7d8ff3a74 97 int j = 0; // Peaks index
vsutardja 12:54e7d8ff3a74 98 int center = 64; // Center estimate
vsutardja 12:54e7d8ff3a74 99 int centers[5] = {0}; // Possible centers
vsutardja 12:54e7d8ff3a74 100 int prev_center = 64; // Previous center
vsutardja 12:54e7d8ff3a74 101 float scores[5] = {0}; // Candidate scores
vsutardja 12:54e7d8ff3a74 102 int best_score_idx = 0; // Most likely center index
vsutardja 0:fcf070a88ba0 103
vsutardja 0:fcf070a88ba0 104 // ================
vsutardja 0:fcf070a88ba0 105 // Functions
vsutardja 0:fcf070a88ba0 106 // ================
vsutardja 0:fcf070a88ba0 107
vsutardja 12:54e7d8ff3a74 108 // Communications
vsutardja 12:54e7d8ff3a74 109 void communication(void const *args) {
vsutardja 11:4348bba086a4 110 telemetry_serial.baud(115200);
vsutardja 11:4348bba086a4 111 telemetry_obj.transmit_header();
vsutardja 9:10fcf3d0ec2d 112 while (true) {
vsutardja 12:54e7d8ff3a74 113 tele_time_ms = t_tele.read_ms();
vsutardja 11:4348bba086a4 114 telemetry_obj.do_io();
vsutardja 9:10fcf3d0ec2d 115 }
vsutardja 9:10fcf3d0ec2d 116 }
vsutardja 11:4348bba086a4 117 //void communication(void const *args) {
vsutardja 12:54e7d8ff3a74 118 // // Initialize bluetooth
vsutardja 12:54e7d8ff3a74 119 // bt.baud(115200);
vsutardja 12:54e7d8ff3a74 120 //
vsutardja 11:4348bba086a4 121 // while (true) {
vsutardja 11:4348bba086a4 122 // bt.printf("\r\nPress q to return to this prompt.\r\n");
vsutardja 11:4348bba086a4 123 // bt.printf("Available diagnostics:\r\n");
vsutardja 11:4348bba086a4 124 // bt.printf(" [0] Velocity\r\n");
vsutardja 11:4348bba086a4 125 // bt.printf(" [1] Steering\r\n");
vsutardja 11:4348bba086a4 126 // bt.printf(" [2] Change Kp\r\n");
vsutardja 11:4348bba086a4 127 // bt.printf(" [3] Change Ki\r\n");
vsutardja 11:4348bba086a4 128 // bt.printf(" [4] Change Kd\r\n");
vsutardja 11:4348bba086a4 129 // bt.printf(" [5] Change Ks\r\n");
vsutardja 11:4348bba086a4 130 // bt.printf(" [6] Change reference velocity\r\n");
vsutardja 12:54e7d8ff3a74 131 // bt.printf(" [7] EMERGENCY STOP\r\n");
vsutardja 12:54e7d8ff3a74 132 // bt.printf(" [8] Timing\r\n");
vsutardja 12:54e7d8ff3a74 133 // comm_cmd = bt.getc();
vsutardja 12:54e7d8ff3a74 134 // while (comm_cmd != 'q') {
vsutardja 12:54e7d8ff3a74 135 // switch(atoi(&comm_cmd)){
vsutardja 11:4348bba086a4 136 // case 0:
vsutardja 12:54e7d8ff3a74 137 // bt.printf("Duty cycle: %f, Pulse count: %d, Velocity: %f, Kp: %f, Ki: %f, Kd: %f\r\n", motor_duty, curr_pulses, velocity, Kp, Ki, Kd);
vsutardja 11:4348bba086a4 138 // break;
vsutardja 11:4348bba086a4 139 // case 1:
vsutardja 12:54e7d8ff3a74 140 // bt.printf("Servo angle: %f, Track center: %d, t_int: %d\r\n", angle, center, t_int);
vsutardja 11:4348bba086a4 141 // break;
vsutardja 11:4348bba086a4 142 // case 2:
vsutardja 11:4348bba086a4 143 // bt.printf("Current: %f, New (5 digits): ", Kp);
vsutardja 11:4348bba086a4 144 // for (int i = 0; i < 5; i++) {
vsutardja 12:54e7d8ff3a74 145 // comm_in[i] = bt.getc();
vsutardja 12:54e7d8ff3a74 146 // bt.putc(comm_in[i]);
vsutardja 11:4348bba086a4 147 // }
vsutardja 11:4348bba086a4 148 // bt.printf("\r\n");
vsutardja 12:54e7d8ff3a74 149 // Kp = atof(comm_in);
vsutardja 11:4348bba086a4 150 // motor_ctrl.setTunings(Kp, Ki, Kd);
vsutardja 12:54e7d8ff3a74 151 // comm_cmd = 'q';
vsutardja 11:4348bba086a4 152 // break;
vsutardja 11:4348bba086a4 153 // case 3:
vsutardja 11:4348bba086a4 154 // bt.printf("Current: %f, New (5 digits): ", Ki);
vsutardja 11:4348bba086a4 155 // for (int i = 0; i < 5; i++) {
vsutardja 12:54e7d8ff3a74 156 // comm_in[i] = bt.getc();
vsutardja 12:54e7d8ff3a74 157 // bt.putc(comm_in[i]);
vsutardja 11:4348bba086a4 158 // }
vsutardja 11:4348bba086a4 159 // bt.printf("\r\n");
vsutardja 12:54e7d8ff3a74 160 // Ki = atof(comm_in);
vsutardja 11:4348bba086a4 161 // motor_ctrl.setTunings(Kp, Ki, Kd);
vsutardja 12:54e7d8ff3a74 162 // comm_cmd = 'q';
vsutardja 11:4348bba086a4 163 // break;
vsutardja 11:4348bba086a4 164 // case 4:
vsutardja 11:4348bba086a4 165 // bt.printf("Current: %f, New (5 digits): ", Kd);
vsutardja 11:4348bba086a4 166 // for (int i = 0; i < 5; i++) {
vsutardja 12:54e7d8ff3a74 167 // comm_in[i] = bt.getc();
vsutardja 12:54e7d8ff3a74 168 // bt.putc(comm_in[i]);
vsutardja 11:4348bba086a4 169 // }
vsutardja 11:4348bba086a4 170 // bt.printf("\r\n");
vsutardja 12:54e7d8ff3a74 171 // Kd = atof(comm_in);
vsutardja 11:4348bba086a4 172 // motor_ctrl.setTunings(Kp, Ki, Kd);
vsutardja 12:54e7d8ff3a74 173 // comm_cmd = 'q';
vsutardja 11:4348bba086a4 174 // break;
vsutardja 11:4348bba086a4 175 // case 5:
vsutardja 11:4348bba086a4 176 // bt.printf("Current: %f, New (5 digits): ", Ks);
vsutardja 11:4348bba086a4 177 // for (int i = 0; i < 5; i++) {
vsutardja 12:54e7d8ff3a74 178 // comm_in[i] = bt.getc();
vsutardja 12:54e7d8ff3a74 179 // bt.putc(comm_in[i]);
vsutardja 11:4348bba086a4 180 // }
vsutardja 11:4348bba086a4 181 // bt.printf("\r\n");
vsutardja 12:54e7d8ff3a74 182 // Ks = atof(comm_in);
vsutardja 12:54e7d8ff3a74 183 // comm_cmd = 'q';
vsutardja 11:4348bba086a4 184 // break;
vsutardja 11:4348bba086a4 185 // case 6:
vsutardja 11:4348bba086a4 186 // bt.printf("Current: %f, New (5 digits): ", ref_v);
vsutardja 11:4348bba086a4 187 // for (int i = 0; i < 5; i++) {
vsutardja 12:54e7d8ff3a74 188 // comm_in[i] = bt.getc();
vsutardja 12:54e7d8ff3a74 189 // bt.putc(comm_in[i]);
vsutardja 11:4348bba086a4 190 // }
vsutardja 11:4348bba086a4 191 // bt.printf("\r\n");
vsutardja 12:54e7d8ff3a74 192 // ref_v = atof(comm_in);
vsutardja 11:4348bba086a4 193 // motor_ctrl.setSetPoint(ref_v);
vsutardja 12:54e7d8ff3a74 194 // comm_cmd = 'q';
vsutardja 12:54e7d8ff3a74 195 // break;
vsutardja 12:54e7d8ff3a74 196 // case 7:
vsutardja 12:54e7d8ff3a74 197 // e_stop = true;
vsutardja 12:54e7d8ff3a74 198 // bt.printf("STOPPED\r\n");
vsutardja 12:54e7d8ff3a74 199 // break;
vsutardja 12:54e7d8ff3a74 200 // case 8:
vsutardja 12:54e7d8ff3a74 201 // bt.printf("Exposure: %dus, Camera Read: %dus, Steering: %dus, Velocity: %dus, Total: %dus\r\n", t_int, t_cam-t_int, t_steer, t_vel, t_cam+t_steer+t_vel);
vsutardja 11:4348bba086a4 202 // break;
vsutardja 11:4348bba086a4 203 // }
vsutardja 11:4348bba086a4 204 // if (bt.readable()) {
vsutardja 12:54e7d8ff3a74 205 // comm_cmd = bt.getc();
vsutardja 11:4348bba086a4 206 // }
vsutardja 11:4348bba086a4 207 // }
vsutardja 11:4348bba086a4 208 // }
vsutardja 11:4348bba086a4 209 //}
vsutardja 11:4348bba086a4 210
vsutardja 12:54e7d8ff3a74 211 void control() {
vsutardja 12:54e7d8ff3a74 212 // Image capture
vsutardja 12:54e7d8ff3a74 213 t.reset();
vsutardja 12:54e7d8ff3a74 214
vsutardja 12:54e7d8ff3a74 215 // Dummy read
vsutardja 12:54e7d8ff3a74 216 clk = 0;
vsutardja 12:54e7d8ff3a74 217 wait_us(1);
vsutardja 12:54e7d8ff3a74 218 si = 1;
vsutardja 12:54e7d8ff3a74 219 wait_us(1);
vsutardja 12:54e7d8ff3a74 220 clk = 1;
vsutardja 12:54e7d8ff3a74 221 wait_us(1);
vsutardja 12:54e7d8ff3a74 222 si = 0;
vsutardja 11:4348bba086a4 223
vsutardja 12:54e7d8ff3a74 224 for (int i = 0; i < 128; i++) {
vsutardja 12:54e7d8ff3a74 225 wait_us(1);
vsutardja 12:54e7d8ff3a74 226 clk = 0;
vsutardja 12:54e7d8ff3a74 227 wait_us(1);
vsutardja 12:54e7d8ff3a74 228 clk = 1;
vsutardja 12:54e7d8ff3a74 229 }
vsutardja 12:54e7d8ff3a74 230
vsutardja 12:54e7d8ff3a74 231 // Expose
vsutardja 12:54e7d8ff3a74 232 wait_us(t_int);
vsutardja 12:54e7d8ff3a74 233
vsutardja 12:54e7d8ff3a74 234 // Read camera
vsutardja 12:54e7d8ff3a74 235 clk = 0;
vsutardja 12:54e7d8ff3a74 236 wait_us(1);
vsutardja 0:fcf070a88ba0 237 si = 1;
vsutardja 0:fcf070a88ba0 238 wait_us(1);
vsutardja 0:fcf070a88ba0 239 clk = 1;
vsutardja 0:fcf070a88ba0 240 wait_us(1);
vsutardja 0:fcf070a88ba0 241 si = 0;
vsutardja 0:fcf070a88ba0 242 wait_us(1);
vsutardja 0:fcf070a88ba0 243
vsutardja 0:fcf070a88ba0 244 for (int i = 0; i < 128; i++) {
vsutardja 0:fcf070a88ba0 245 clk = 0;
vsutardja 12:54e7d8ff3a74 246 if (i > 9 && i < 118) {
vsutardja 12:54e7d8ff3a74 247 img[i-10] = ao.read_u16();
vsutardja 12:54e7d8ff3a74 248 tele_linescan[i-10] = img[i-10];
vsutardja 12:54e7d8ff3a74 249 } else {
vsutardja 12:54e7d8ff3a74 250 wait_us(1);
vsutardja 12:54e7d8ff3a74 251 }
vsutardja 0:fcf070a88ba0 252 clk = 1;
vsutardja 0:fcf070a88ba0 253 wait_us(1);
vsutardja 0:fcf070a88ba0 254 }
vsutardja 12:54e7d8ff3a74 255
vsutardja 12:54e7d8ff3a74 256 t_cam = t.read_us();
vsutardja 0:fcf070a88ba0 257
vsutardja 12:54e7d8ff3a74 258 // Steering control
vsutardja 12:54e7d8ff3a74 259 t.reset();
vsutardja 0:fcf070a88ba0 260
vsutardja 12:54e7d8ff3a74 261 // Detect peak edges
vsutardja 12:54e7d8ff3a74 262 j = 0;
vsutardja 12:54e7d8ff3a74 263 for (int i = 0; i < 107 && j < 5; i++) {
vsutardja 12:54e7d8ff3a74 264 if (img[i] > 45000) {
vsutardja 12:54e7d8ff3a74 265 left[j] = i;
vsutardja 12:54e7d8ff3a74 266 while (img[i] > 45000) {
vsutardja 12:54e7d8ff3a74 267 i = i + 1;
vsutardja 0:fcf070a88ba0 268 }
vsutardja 12:54e7d8ff3a74 269 right[j] = i;
vsutardja 12:54e7d8ff3a74 270 j = j + 1;
vsutardja 0:fcf070a88ba0 271 }
vsutardja 0:fcf070a88ba0 272 }
vsutardja 0:fcf070a88ba0 273
vsutardja 12:54e7d8ff3a74 274 // Calculate peak centers
vsutardja 12:54e7d8ff3a74 275 for (int i = 0; i < j; i++) {
vsutardja 12:54e7d8ff3a74 276 centers[i] = (left[i] + right[i] + 10) / 2;
vsutardja 12:54e7d8ff3a74 277 }
vsutardja 12:54e7d8ff3a74 278
vsutardja 12:54e7d8ff3a74 279 // Assign scores
vsutardja 12:54e7d8ff3a74 280 for (int i = 0; i < j; i++) {
vsutardja 12:54e7d8ff3a74 281 scores[i] = 10 / (right[i] - left[i]) + img[centers[i]] / 65536 + 10 / abs(centers[i] - prev_center);
vsutardja 3:c57674c348bd 282 }
vsutardja 3:c57674c348bd 283
vsutardja 12:54e7d8ff3a74 284 // Choose most likely center
vsutardja 12:54e7d8ff3a74 285 best_score_idx = 0;
vsutardja 12:54e7d8ff3a74 286 for (int i = 0; i < j; i++) {
vsutardja 12:54e7d8ff3a74 287 if (scores[i] > scores[best_score_idx]) {
vsutardja 12:54e7d8ff3a74 288 best_score_idx = i;
vsutardja 12:54e7d8ff3a74 289 }
vsutardja 12:54e7d8ff3a74 290 }
vsutardja 12:54e7d8ff3a74 291 prev_center = center;
vsutardja 12:54e7d8ff3a74 292 center = centers[best_score_idx];
vsutardja 12:54e7d8ff3a74 293 tele_center = center;
vsutardja 3:c57674c348bd 294
vsutardja 12:54e7d8ff3a74 295 // Set servo angle
vsutardja 12:54e7d8ff3a74 296 angle = 88 + (55 - center) * Ks;
vsutardja 12:54e7d8ff3a74 297 if (angle > 113) {
vsutardja 12:54e7d8ff3a74 298 angle = 113;
vsutardja 12:54e7d8ff3a74 299 }
vsutardja 12:54e7d8ff3a74 300 if (angle < 63) {
vsutardja 12:54e7d8ff3a74 301 angle = 63;
vsutardja 12:54e7d8ff3a74 302 }
vsutardja 12:54e7d8ff3a74 303 servo = angle / 180;
vsutardja 12:54e7d8ff3a74 304
vsutardja 12:54e7d8ff3a74 305 // AGC
vsutardja 12:54e7d8ff3a74 306 max = -1;
vsutardja 12:54e7d8ff3a74 307 for (int i = 0; i < 107; i++) {
vsutardja 12:54e7d8ff3a74 308 if (img[i] > max) {
vsutardja 12:54e7d8ff3a74 309 max = img[i];
vsutardja 12:54e7d8ff3a74 310 }
vsutardja 12:54e7d8ff3a74 311 }
vsutardja 11:4348bba086a4 312 if (max > 60000) {
vsutardja 11:4348bba086a4 313 t_int = t_int - 0.1 * (max - 60000);
vsutardja 11:4348bba086a4 314 }
vsutardja 11:4348bba086a4 315 if (max < 50000) {
vsutardja 11:4348bba086a4 316 t_int = t_int + 0.1 * (50000 - max);
vsutardja 11:4348bba086a4 317 }
vsutardja 12:54e7d8ff3a74 318 if (t_int < T_INT_MIN) {
vsutardja 12:54e7d8ff3a74 319 t_int = T_INT_MIN;
vsutardja 12:54e7d8ff3a74 320 }
vsutardja 12:54e7d8ff3a74 321 if (t_int > T_INT_MAX) {
vsutardja 12:54e7d8ff3a74 322 t_int = T_INT_MAX;
vsutardja 11:4348bba086a4 323 }
vsutardja 12:54e7d8ff3a74 324 tele_exposure = t_int;
vsutardja 11:4348bba086a4 325
vsutardja 12:54e7d8ff3a74 326 t_steer = t.read_us();
vsutardja 3:c57674c348bd 327
vsutardja 12:54e7d8ff3a74 328 // wait_us(8000 - t.read_us());
vsutardja 11:4348bba086a4 329
vsutardja 12:54e7d8ff3a74 330 // Velocity control
vsutardja 12:54e7d8ff3a74 331 t.reset();
vsutardja 12:54e7d8ff3a74 332 if (!e_stop) {
vsutardja 12:54e7d8ff3a74 333 curr_pulses = qei.getPulses();
vsutardja 12:54e7d8ff3a74 334 velocity = (curr_pulses - prev_pulses) / interrupt_T / ppr * c;
vsutardja 12:54e7d8ff3a74 335 tele_velocity = velocity;
vsutardja 12:54e7d8ff3a74 336 prev_pulses = curr_pulses;
vsutardja 12:54e7d8ff3a74 337 motor_ctrl.setProcessValue(velocity);
vsutardja 12:54e7d8ff3a74 338 motor_duty = motor_ctrl.compute();
vsutardja 12:54e7d8ff3a74 339 motor = 1.0 - motor_duty;
vsutardja 12:54e7d8ff3a74 340 tele_pwm = motor_duty;
vsutardja 0:fcf070a88ba0 341 }
vsutardja 12:54e7d8ff3a74 342 t_vel = t.read_us();
vsutardja 0:fcf070a88ba0 343 }
vsutardja 0:fcf070a88ba0 344
vsutardja 0:fcf070a88ba0 345 // ====
vsutardja 0:fcf070a88ba0 346 // Main
vsutardja 0:fcf070a88ba0 347 // ====
vsutardja 0:fcf070a88ba0 348 int main() {
vsutardja 11:4348bba086a4 349 t.start();
vsutardja 12:54e7d8ff3a74 350 t_tele.start();
vsutardja 12:54e7d8ff3a74 351 tele_center.set_limits(0, 128);
vsutardja 12:54e7d8ff3a74 352 tele_pwm.set_limits(0.0, 1.0);
vsutardja 2:a8adff46eaca 353
vsutardja 0:fcf070a88ba0 354 // Initialize motor
vsutardja 12:54e7d8ff3a74 355 motor.period_us(motor_T);
vsutardja 12:54e7d8ff3a74 356 motor = 1.0 - motor_duty;
vsutardja 0:fcf070a88ba0 357
vsutardja 0:fcf070a88ba0 358 // Initialize motor controller
vsutardja 0:fcf070a88ba0 359 motor_ctrl.setInputLimits(0.0, 10.0);
vsutardja 4:947c3634b649 360 motor_ctrl.setOutputLimits(0.01, 0.5);
vsutardja 4:947c3634b649 361 motor_ctrl.setSetPoint(ref_v);
vsutardja 0:fcf070a88ba0 362 motor_ctrl.setBias(0.0);
vsutardja 0:fcf070a88ba0 363 motor_ctrl.setMode(1);
vsutardja 0:fcf070a88ba0 364
vsutardja 12:54e7d8ff3a74 365 // Initialize servo
vsutardja 12:54e7d8ff3a74 366 servo.calibrate(0.001, 45.0);
vsutardja 12:54e7d8ff3a74 367 servo = angle / 180.0;
vsutardja 0:fcf070a88ba0 368
vsutardja 0:fcf070a88ba0 369 // Initialize communications thread
vsutardja 12:54e7d8ff3a74 370 Thread communication_thread(communication);
vsutardja 0:fcf070a88ba0 371
vsutardja 12:54e7d8ff3a74 372 control_interrupt.attach(control, interrupt_T);
vsutardja 12:54e7d8ff3a74 373
vsutardja 12:54e7d8ff3a74 374 while (true);
vsutardja 0:fcf070a88ba0 375 }