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@13:d6e167698517, 2016-04-08 (annotated)

Committer:
vsutardja
Date:
Fri Apr 08 23:43:12 2016 +0000
Revision:
13:d6e167698517
Parent:
12:54e7d8ff3a74
Child:
14:a0614f48e6ef
Faster camera reads with direct register writes

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 13:d6e167698517 45 //void kill(Arguments *input, Reply *output);
vsutardja 13:d6e167698517 46 //RPCFunction rpc_kill(&kill, "kill");
vsutardja 13:d6e167698517 47
vsutardja 0:fcf070a88ba0 48 // =====
vsutardja 0:fcf070a88ba0 49 // Motor
vsutardja 0:fcf070a88ba0 50 // =====
vsutardja 12:54e7d8ff3a74 51 const int motor_T = 25000; // Frequency
vsutardja 12:54e7d8ff3a74 52 float motor_duty = 0.0; // Duty cycle
vsutardja 12:54e7d8ff3a74 53 bool e_stop = false; // Emergency stop
vsutardja 12:54e7d8ff3a74 54 PwmOut motor(PTA4); // Enable pin (PWM)
vsutardja 0:fcf070a88ba0 55
vsutardja 0:fcf070a88ba0 56 // =======
vsutardja 0:fcf070a88ba0 57 // Encoder
vsutardja 0:fcf070a88ba0 58 // =======
vsutardja 12:54e7d8ff3a74 59 const int ppr = 389; // Pulses per revolution
vsutardja 12:54e7d8ff3a74 60 const float c = 0.20106; // Wheel circumference
vsutardja 12:54e7d8ff3a74 61 int prev_pulses = 0; // Previous pulse count
vsutardja 12:54e7d8ff3a74 62 int curr_pulses = 0; // Current pulse count
vsutardja 12:54e7d8ff3a74 63 float velocity = 0; // Velocity
vsutardja 12:54e7d8ff3a74 64 QEI qei(PTD3, PTD2, NC, ppr, QEI::X4_ENCODING); // Quadrature encoder
vsutardja 0:fcf070a88ba0 65
vsutardja 0:fcf070a88ba0 66 // ========
vsutardja 0:fcf070a88ba0 67 // Velocity
vsutardja 0:fcf070a88ba0 68 // ========
vsutardja 12:54e7d8ff3a74 69 float Kp = 6.0; // Proportional factor
vsutardja 12:54e7d8ff3a74 70 float Ki = 0; // Integral factor
vsutardja 12:54e7d8ff3a74 71 float Kd = 0; // Derivative factor
vsutardja 12:54e7d8ff3a74 72 float interval = 0.01; // Sampling interval
vsutardja 13:d6e167698517 73 float ref_v = 0.5; // Reference velocity
vsutardja 12:54e7d8ff3a74 74 PID motor_ctrl(Kp, Ki, Kd, interval); // Motor controller
vsutardja 0:fcf070a88ba0 75
vsutardja 0:fcf070a88ba0 76 // =====
vsutardja 0:fcf070a88ba0 77 // Servo
vsutardja 0:fcf070a88ba0 78 // =====
vsutardja 12:54e7d8ff3a74 79 float angle = 88; // Angle
vsutardja 12:54e7d8ff3a74 80 float Ks = 0.9; // Steering proportion
vsutardja 12:54e7d8ff3a74 81 Servo servo(PTA12); // Signal pin (PWM)
vsutardja 0:fcf070a88ba0 82
vsutardja 0:fcf070a88ba0 83 // ======
vsutardja 0:fcf070a88ba0 84 // Camera
vsutardja 0:fcf070a88ba0 85 // ======
vsutardja 12:54e7d8ff3a74 86 int t_int = 10000; // Exposure time
vsutardja 13:d6e167698517 87 const int T_INT_MAX = interrupt_T * 1000000 - 1000; // Maximum exposure time
vsutardja 12:54e7d8ff3a74 88 const int T_INT_MIN = 35; // Minimum exposure time
vsutardja 12:54e7d8ff3a74 89 int img[108] = {0}; // Image data
vsutardja 12:54e7d8ff3a74 90 DigitalOut clk(PTD5); // Clock pin
vsutardja 12:54e7d8ff3a74 91 DigitalOut si(PTD0); // SI pin
vsutardja 12:54e7d8ff3a74 92 FastAnalogIn ao(PTC2); // AO pin
vsutardja 0:fcf070a88ba0 93
vsutardja 0:fcf070a88ba0 94 // ================
vsutardja 0:fcf070a88ba0 95 // Image processing
vsutardja 0:fcf070a88ba0 96 // ================
vsutardja 12:54e7d8ff3a74 97 int max = -1; // Maximum luminosity
vsutardja 12:54e7d8ff3a74 98 int left[5] = {0}; // Left edge
vsutardja 12:54e7d8ff3a74 99 int right[5] = {0}; // Right edge
vsutardja 12:54e7d8ff3a74 100 int j = 0; // Peaks index
vsutardja 12:54e7d8ff3a74 101 int center = 64; // Center estimate
vsutardja 12:54e7d8ff3a74 102 int centers[5] = {0}; // Possible centers
vsutardja 12:54e7d8ff3a74 103 int prev_center = 64; // Previous center
vsutardja 12:54e7d8ff3a74 104 float scores[5] = {0}; // Candidate scores
vsutardja 12:54e7d8ff3a74 105 int best_score_idx = 0; // Most likely center index
vsutardja 0:fcf070a88ba0 106
vsutardja 0:fcf070a88ba0 107 // ================
vsutardja 0:fcf070a88ba0 108 // Functions
vsutardja 0:fcf070a88ba0 109 // ================
vsutardja 0:fcf070a88ba0 110
vsutardja 13:d6e167698517 111
vsutardja 13:d6e167698517 112 void killswitch(MODSERIAL_IRQ_INFO *q) {
vsutardja 13:d6e167698517 113 MODSERIAL *serial = q->serial;
vsutardja 13:d6e167698517 114 if (serial->rxGetLastChar() == 'k') {
vsutardja 13:d6e167698517 115 e_stop = true;
vsutardja 13:d6e167698517 116 motor = 1.0;
vsutardja 13:d6e167698517 117 }
vsutardja 13:d6e167698517 118 if (serial->rxGetLastChar() == '+') {
vsutardja 13:d6e167698517 119 ref_v = ref_v + 0.2;
vsutardja 13:d6e167698517 120 motor_ctrl.setSetPoint(ref_v);
vsutardja 13:d6e167698517 121 }
vsutardja 13:d6e167698517 122 if (serial->rxGetLastChar() == '-') {
vsutardja 13:d6e167698517 123 ref_v = ref_v - 0.2;
vsutardja 13:d6e167698517 124 motor_ctrl.setSetPoint(ref_v);
vsutardja 13:d6e167698517 125 }
vsutardja 13:d6e167698517 126 }
vsutardja 13:d6e167698517 127
vsutardja 12:54e7d8ff3a74 128 // Communications
vsutardja 12:54e7d8ff3a74 129 void communication(void const *args) {
vsutardja 11:4348bba086a4 130 telemetry_serial.baud(115200);
vsutardja 13:d6e167698517 131 telemetry_serial.attach(&killswitch, MODSERIAL::RxIrq);
vsutardja 11:4348bba086a4 132 telemetry_obj.transmit_header();
vsutardja 9:10fcf3d0ec2d 133 while (true) {
vsutardja 12:54e7d8ff3a74 134 tele_time_ms = t_tele.read_ms();
vsutardja 11:4348bba086a4 135 telemetry_obj.do_io();
vsutardja 9:10fcf3d0ec2d 136 }
vsutardja 9:10fcf3d0ec2d 137 }
vsutardja 13:d6e167698517 138
vsutardja 13:d6e167698517 139 //void kill(Arguments *input, Reply *output) {
vsutardja 13:d6e167698517 140 // output->putData("E_STOP\r\n");
vsutardja 13:d6e167698517 141 // motor = 1.0;
vsutardja 13:d6e167698517 142 // e_stop = true;
vsutardja 13:d6e167698517 143 //}
vsutardja 13:d6e167698517 144
vsutardja 11:4348bba086a4 145 //void communication(void const *args) {
vsutardja 12:54e7d8ff3a74 146 // // Initialize bluetooth
vsutardja 12:54e7d8ff3a74 147 // bt.baud(115200);
vsutardja 12:54e7d8ff3a74 148 //
vsutardja 11:4348bba086a4 149 // while (true) {
vsutardja 11:4348bba086a4 150 // bt.printf("\r\nPress q to return to this prompt.\r\n");
vsutardja 11:4348bba086a4 151 // bt.printf("Available diagnostics:\r\n");
vsutardja 11:4348bba086a4 152 // bt.printf(" [0] Velocity\r\n");
vsutardja 11:4348bba086a4 153 // bt.printf(" [1] Steering\r\n");
vsutardja 11:4348bba086a4 154 // bt.printf(" [2] Change Kp\r\n");
vsutardja 11:4348bba086a4 155 // bt.printf(" [3] Change Ki\r\n");
vsutardja 11:4348bba086a4 156 // bt.printf(" [4] Change Kd\r\n");
vsutardja 11:4348bba086a4 157 // bt.printf(" [5] Change Ks\r\n");
vsutardja 11:4348bba086a4 158 // bt.printf(" [6] Change reference velocity\r\n");
vsutardja 12:54e7d8ff3a74 159 // bt.printf(" [7] EMERGENCY STOP\r\n");
vsutardja 12:54e7d8ff3a74 160 // bt.printf(" [8] Timing\r\n");
vsutardja 12:54e7d8ff3a74 161 // comm_cmd = bt.getc();
vsutardja 12:54e7d8ff3a74 162 // while (comm_cmd != 'q') {
vsutardja 12:54e7d8ff3a74 163 // switch(atoi(&comm_cmd)){
vsutardja 11:4348bba086a4 164 // case 0:
vsutardja 12:54e7d8ff3a74 165 // 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 166 // break;
vsutardja 11:4348bba086a4 167 // case 1:
vsutardja 12:54e7d8ff3a74 168 // bt.printf("Servo angle: %f, Track center: %d, t_int: %d\r\n", angle, center, t_int);
vsutardja 11:4348bba086a4 169 // break;
vsutardja 11:4348bba086a4 170 // case 2:
vsutardja 11:4348bba086a4 171 // bt.printf("Current: %f, New (5 digits): ", Kp);
vsutardja 11:4348bba086a4 172 // for (int i = 0; i < 5; i++) {
vsutardja 12:54e7d8ff3a74 173 // comm_in[i] = bt.getc();
vsutardja 12:54e7d8ff3a74 174 // bt.putc(comm_in[i]);
vsutardja 11:4348bba086a4 175 // }
vsutardja 11:4348bba086a4 176 // bt.printf("\r\n");
vsutardja 12:54e7d8ff3a74 177 // Kp = atof(comm_in);
vsutardja 11:4348bba086a4 178 // motor_ctrl.setTunings(Kp, Ki, Kd);
vsutardja 12:54e7d8ff3a74 179 // comm_cmd = 'q';
vsutardja 11:4348bba086a4 180 // break;
vsutardja 11:4348bba086a4 181 // case 3:
vsutardja 11:4348bba086a4 182 // bt.printf("Current: %f, New (5 digits): ", Ki);
vsutardja 11:4348bba086a4 183 // for (int i = 0; i < 5; i++) {
vsutardja 12:54e7d8ff3a74 184 // comm_in[i] = bt.getc();
vsutardja 12:54e7d8ff3a74 185 // bt.putc(comm_in[i]);
vsutardja 11:4348bba086a4 186 // }
vsutardja 11:4348bba086a4 187 // bt.printf("\r\n");
vsutardja 12:54e7d8ff3a74 188 // Ki = atof(comm_in);
vsutardja 11:4348bba086a4 189 // motor_ctrl.setTunings(Kp, Ki, Kd);
vsutardja 12:54e7d8ff3a74 190 // comm_cmd = 'q';
vsutardja 11:4348bba086a4 191 // break;
vsutardja 11:4348bba086a4 192 // case 4:
vsutardja 11:4348bba086a4 193 // bt.printf("Current: %f, New (5 digits): ", Kd);
vsutardja 11:4348bba086a4 194 // for (int i = 0; i < 5; i++) {
vsutardja 12:54e7d8ff3a74 195 // comm_in[i] = bt.getc();
vsutardja 12:54e7d8ff3a74 196 // bt.putc(comm_in[i]);
vsutardja 11:4348bba086a4 197 // }
vsutardja 11:4348bba086a4 198 // bt.printf("\r\n");
vsutardja 12:54e7d8ff3a74 199 // Kd = atof(comm_in);
vsutardja 11:4348bba086a4 200 // motor_ctrl.setTunings(Kp, Ki, Kd);
vsutardja 12:54e7d8ff3a74 201 // comm_cmd = 'q';
vsutardja 11:4348bba086a4 202 // break;
vsutardja 11:4348bba086a4 203 // case 5:
vsutardja 11:4348bba086a4 204 // bt.printf("Current: %f, New (5 digits): ", Ks);
vsutardja 11:4348bba086a4 205 // for (int i = 0; i < 5; i++) {
vsutardja 12:54e7d8ff3a74 206 // comm_in[i] = bt.getc();
vsutardja 12:54e7d8ff3a74 207 // bt.putc(comm_in[i]);
vsutardja 11:4348bba086a4 208 // }
vsutardja 11:4348bba086a4 209 // bt.printf("\r\n");
vsutardja 12:54e7d8ff3a74 210 // Ks = atof(comm_in);
vsutardja 12:54e7d8ff3a74 211 // comm_cmd = 'q';
vsutardja 11:4348bba086a4 212 // break;
vsutardja 11:4348bba086a4 213 // case 6:
vsutardja 11:4348bba086a4 214 // bt.printf("Current: %f, New (5 digits): ", ref_v);
vsutardja 11:4348bba086a4 215 // for (int i = 0; i < 5; i++) {
vsutardja 12:54e7d8ff3a74 216 // comm_in[i] = bt.getc();
vsutardja 12:54e7d8ff3a74 217 // bt.putc(comm_in[i]);
vsutardja 11:4348bba086a4 218 // }
vsutardja 11:4348bba086a4 219 // bt.printf("\r\n");
vsutardja 12:54e7d8ff3a74 220 // ref_v = atof(comm_in);
vsutardja 11:4348bba086a4 221 // motor_ctrl.setSetPoint(ref_v);
vsutardja 12:54e7d8ff3a74 222 // comm_cmd = 'q';
vsutardja 12:54e7d8ff3a74 223 // break;
vsutardja 12:54e7d8ff3a74 224 // case 7:
vsutardja 12:54e7d8ff3a74 225 // e_stop = true;
vsutardja 12:54e7d8ff3a74 226 // bt.printf("STOPPED\r\n");
vsutardja 12:54e7d8ff3a74 227 // break;
vsutardja 12:54e7d8ff3a74 228 // case 8:
vsutardja 12:54e7d8ff3a74 229 // 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 230 // break;
vsutardja 11:4348bba086a4 231 // }
vsutardja 11:4348bba086a4 232 // if (bt.readable()) {
vsutardja 12:54e7d8ff3a74 233 // comm_cmd = bt.getc();
vsutardja 11:4348bba086a4 234 // }
vsutardja 11:4348bba086a4 235 // }
vsutardja 11:4348bba086a4 236 // }
vsutardja 11:4348bba086a4 237 //}
vsutardja 11:4348bba086a4 238
vsutardja 12:54e7d8ff3a74 239 void control() {
vsutardja 12:54e7d8ff3a74 240 // Image capture
vsutardja 12:54e7d8ff3a74 241 t.reset();
vsutardja 12:54e7d8ff3a74 242
vsutardja 12:54e7d8ff3a74 243 // Dummy read
vsutardja 13:d6e167698517 244 // clk = 0;
vsutardja 13:d6e167698517 245 PTD->PCOR = (0x01 << 5);
vsutardja 13:d6e167698517 246 // j = 0;
vsutardja 13:d6e167698517 247 // wait_us(1);
vsutardja 13:d6e167698517 248 // si = 1;
vsutardja 13:d6e167698517 249 PTD->PSOR = (0x01);
vsutardja 13:d6e167698517 250 // j= 0;
vsutardja 13:d6e167698517 251 // wait_us(1);
vsutardja 13:d6e167698517 252 // clk = 1;
vsutardja 13:d6e167698517 253 PTD->PSOR = (0x01 << 5);
vsutardja 13:d6e167698517 254 // j = 0;
vsutardja 13:d6e167698517 255 // wait_us(1);
vsutardja 13:d6e167698517 256 // si = 0;
vsutardja 13:d6e167698517 257 PTD->PCOR = (0x01);
vsutardja 13:d6e167698517 258 // j = 0;
vsutardja 11:4348bba086a4 259
vsutardja 12:54e7d8ff3a74 260 for (int i = 0; i < 128; i++) {
vsutardja 13:d6e167698517 261 // wait_us(1);
vsutardja 13:d6e167698517 262 PTD->PCOR = (0x01 << 5);
vsutardja 13:d6e167698517 263 // j = 0;
vsutardja 13:d6e167698517 264 // wait_us(1);
vsutardja 13:d6e167698517 265 PTD->PSOR = (0x01 << 5);
vsutardja 13:d6e167698517 266 // j = 0;
vsutardja 12:54e7d8ff3a74 267 }
vsutardja 12:54e7d8ff3a74 268
vsutardja 12:54e7d8ff3a74 269 // Expose
vsutardja 12:54e7d8ff3a74 270 wait_us(t_int);
vsutardja 12:54e7d8ff3a74 271
vsutardja 12:54e7d8ff3a74 272 // Read camera
vsutardja 13:d6e167698517 273 // clk = 0;
vsutardja 13:d6e167698517 274 PTD->PCOR = (0x01 << 5);
vsutardja 13:d6e167698517 275 // j = 0;
vsutardja 13:d6e167698517 276 // wait_us(1);
vsutardja 13:d6e167698517 277 // si = 1;
vsutardja 13:d6e167698517 278 PTD->PSOR = (0x01);
vsutardja 13:d6e167698517 279 // j = 0;
vsutardja 13:d6e167698517 280 // wait_us(1);
vsutardja 13:d6e167698517 281 // clk = 1;
vsutardja 13:d6e167698517 282 PTD->PSOR = (0x01 << 5);
vsutardja 13:d6e167698517 283 // j = 0;
vsutardja 13:d6e167698517 284 // wait_us(1);
vsutardja 13:d6e167698517 285 // si = 0;
vsutardja 13:d6e167698517 286 PTD->PCOR = (0x01);
vsutardja 13:d6e167698517 287 // j = 0;
vsutardja 13:d6e167698517 288 // wait_us(1);
vsutardja 0:fcf070a88ba0 289
vsutardja 0:fcf070a88ba0 290 for (int i = 0; i < 128; i++) {
vsutardja 13:d6e167698517 291 // clk = 0;
vsutardja 13:d6e167698517 292 PTD->PCOR = (0x01 << 5);
vsutardja 13:d6e167698517 293 // j = 0;
vsutardja 12:54e7d8ff3a74 294 if (i > 9 && i < 118) {
vsutardja 12:54e7d8ff3a74 295 img[i-10] = ao.read_u16();
vsutardja 12:54e7d8ff3a74 296 tele_linescan[i-10] = img[i-10];
vsutardja 12:54e7d8ff3a74 297 }
vsutardja 13:d6e167698517 298 //} else {
vsutardja 13:d6e167698517 299 // wait_us(1);
vsutardja 13:d6e167698517 300 // }
vsutardja 13:d6e167698517 301 PTD->PSOR = (0x01 << 5);
vsutardja 13:d6e167698517 302 // j = 0;
vsutardja 0:fcf070a88ba0 303 }
vsutardja 12:54e7d8ff3a74 304
vsutardja 12:54e7d8ff3a74 305 t_cam = t.read_us();
vsutardja 0:fcf070a88ba0 306
vsutardja 12:54e7d8ff3a74 307 // Steering control
vsutardja 12:54e7d8ff3a74 308 t.reset();
vsutardja 0:fcf070a88ba0 309
vsutardja 12:54e7d8ff3a74 310 // Detect peak edges
vsutardja 12:54e7d8ff3a74 311 j = 0;
vsutardja 12:54e7d8ff3a74 312 for (int i = 0; i < 107 && j < 5; i++) {
vsutardja 12:54e7d8ff3a74 313 if (img[i] > 45000) {
vsutardja 12:54e7d8ff3a74 314 left[j] = i;
vsutardja 12:54e7d8ff3a74 315 while (img[i] > 45000) {
vsutardja 12:54e7d8ff3a74 316 i = i + 1;
vsutardja 0:fcf070a88ba0 317 }
vsutardja 12:54e7d8ff3a74 318 right[j] = i;
vsutardja 12:54e7d8ff3a74 319 j = j + 1;
vsutardja 0:fcf070a88ba0 320 }
vsutardja 0:fcf070a88ba0 321 }
vsutardja 0:fcf070a88ba0 322
vsutardja 12:54e7d8ff3a74 323 // Calculate peak centers
vsutardja 12:54e7d8ff3a74 324 for (int i = 0; i < j; i++) {
vsutardja 12:54e7d8ff3a74 325 centers[i] = (left[i] + right[i] + 10) / 2;
vsutardja 12:54e7d8ff3a74 326 }
vsutardja 12:54e7d8ff3a74 327
vsutardja 12:54e7d8ff3a74 328 // Assign scores
vsutardja 12:54e7d8ff3a74 329 for (int i = 0; i < j; i++) {
vsutardja 12:54e7d8ff3a74 330 scores[i] = 10 / (right[i] - left[i]) + img[centers[i]] / 65536 + 10 / abs(centers[i] - prev_center);
vsutardja 3:c57674c348bd 331 }
vsutardja 3:c57674c348bd 332
vsutardja 12:54e7d8ff3a74 333 // Choose most likely center
vsutardja 12:54e7d8ff3a74 334 best_score_idx = 0;
vsutardja 12:54e7d8ff3a74 335 for (int i = 0; i < j; i++) {
vsutardja 12:54e7d8ff3a74 336 if (scores[i] > scores[best_score_idx]) {
vsutardja 12:54e7d8ff3a74 337 best_score_idx = i;
vsutardja 12:54e7d8ff3a74 338 }
vsutardja 12:54e7d8ff3a74 339 }
vsutardja 12:54e7d8ff3a74 340 prev_center = center;
vsutardja 12:54e7d8ff3a74 341 center = centers[best_score_idx];
vsutardja 12:54e7d8ff3a74 342 tele_center = center;
vsutardja 3:c57674c348bd 343
vsutardja 12:54e7d8ff3a74 344 // Set servo angle
vsutardja 12:54e7d8ff3a74 345 angle = 88 + (55 - center) * Ks;
vsutardja 12:54e7d8ff3a74 346 if (angle > 113) {
vsutardja 12:54e7d8ff3a74 347 angle = 113;
vsutardja 12:54e7d8ff3a74 348 }
vsutardja 12:54e7d8ff3a74 349 if (angle < 63) {
vsutardja 12:54e7d8ff3a74 350 angle = 63;
vsutardja 12:54e7d8ff3a74 351 }
vsutardja 12:54e7d8ff3a74 352 servo = angle / 180;
vsutardja 12:54e7d8ff3a74 353
vsutardja 12:54e7d8ff3a74 354 // AGC
vsutardja 12:54e7d8ff3a74 355 max = -1;
vsutardja 12:54e7d8ff3a74 356 for (int i = 0; i < 107; i++) {
vsutardja 12:54e7d8ff3a74 357 if (img[i] > max) {
vsutardja 12:54e7d8ff3a74 358 max = img[i];
vsutardja 12:54e7d8ff3a74 359 }
vsutardja 12:54e7d8ff3a74 360 }
vsutardja 11:4348bba086a4 361 if (max > 60000) {
vsutardja 11:4348bba086a4 362 t_int = t_int - 0.1 * (max - 60000);
vsutardja 11:4348bba086a4 363 }
vsutardja 11:4348bba086a4 364 if (max < 50000) {
vsutardja 11:4348bba086a4 365 t_int = t_int + 0.1 * (50000 - max);
vsutardja 11:4348bba086a4 366 }
vsutardja 12:54e7d8ff3a74 367 if (t_int < T_INT_MIN) {
vsutardja 12:54e7d8ff3a74 368 t_int = T_INT_MIN;
vsutardja 12:54e7d8ff3a74 369 }
vsutardja 12:54e7d8ff3a74 370 if (t_int > T_INT_MAX) {
vsutardja 12:54e7d8ff3a74 371 t_int = T_INT_MAX;
vsutardja 11:4348bba086a4 372 }
vsutardja 12:54e7d8ff3a74 373 tele_exposure = t_int;
vsutardja 11:4348bba086a4 374
vsutardja 12:54e7d8ff3a74 375 t_steer = t.read_us();
vsutardja 3:c57674c348bd 376
vsutardja 12:54e7d8ff3a74 377 // wait_us(8000 - t.read_us());
vsutardja 11:4348bba086a4 378
vsutardja 12:54e7d8ff3a74 379 // Velocity control
vsutardja 12:54e7d8ff3a74 380 t.reset();
vsutardja 12:54e7d8ff3a74 381 if (!e_stop) {
vsutardja 12:54e7d8ff3a74 382 curr_pulses = qei.getPulses();
vsutardja 12:54e7d8ff3a74 383 velocity = (curr_pulses - prev_pulses) / interrupt_T / ppr * c;
vsutardja 12:54e7d8ff3a74 384 tele_velocity = velocity;
vsutardja 12:54e7d8ff3a74 385 prev_pulses = curr_pulses;
vsutardja 12:54e7d8ff3a74 386 motor_ctrl.setProcessValue(velocity);
vsutardja 12:54e7d8ff3a74 387 motor_duty = motor_ctrl.compute();
vsutardja 12:54e7d8ff3a74 388 motor = 1.0 - motor_duty;
vsutardja 12:54e7d8ff3a74 389 tele_pwm = motor_duty;
vsutardja 13:d6e167698517 390 } else {
vsutardja 13:d6e167698517 391 motor = 1.0;
vsutardja 0:fcf070a88ba0 392 }
vsutardja 12:54e7d8ff3a74 393 t_vel = t.read_us();
vsutardja 0:fcf070a88ba0 394 }
vsutardja 0:fcf070a88ba0 395
vsutardja 0:fcf070a88ba0 396 // ====
vsutardja 0:fcf070a88ba0 397 // Main
vsutardja 0:fcf070a88ba0 398 // ====
vsutardja 0:fcf070a88ba0 399 int main() {
vsutardja 11:4348bba086a4 400 t.start();
vsutardja 12:54e7d8ff3a74 401 t_tele.start();
vsutardja 12:54e7d8ff3a74 402 tele_center.set_limits(0, 128);
vsutardja 12:54e7d8ff3a74 403 tele_pwm.set_limits(0.0, 1.0);
vsutardja 2:a8adff46eaca 404
vsutardja 0:fcf070a88ba0 405 // Initialize motor
vsutardja 12:54e7d8ff3a74 406 motor.period_us(motor_T);
vsutardja 12:54e7d8ff3a74 407 motor = 1.0 - motor_duty;
vsutardja 0:fcf070a88ba0 408
vsutardja 0:fcf070a88ba0 409 // Initialize motor controller
vsutardja 0:fcf070a88ba0 410 motor_ctrl.setInputLimits(0.0, 10.0);
vsutardja 4:947c3634b649 411 motor_ctrl.setOutputLimits(0.01, 0.5);
vsutardja 4:947c3634b649 412 motor_ctrl.setSetPoint(ref_v);
vsutardja 0:fcf070a88ba0 413 motor_ctrl.setBias(0.0);
vsutardja 0:fcf070a88ba0 414 motor_ctrl.setMode(1);
vsutardja 0:fcf070a88ba0 415
vsutardja 12:54e7d8ff3a74 416 // Initialize servo
vsutardja 12:54e7d8ff3a74 417 servo.calibrate(0.001, 45.0);
vsutardja 12:54e7d8ff3a74 418 servo = angle / 180.0;
vsutardja 0:fcf070a88ba0 419
vsutardja 0:fcf070a88ba0 420 // Initialize communications thread
vsutardja 12:54e7d8ff3a74 421 Thread communication_thread(communication);
vsutardja 0:fcf070a88ba0 422
vsutardja 12:54e7d8ff3a74 423 control_interrupt.attach(control, interrupt_T);
vsutardja 12:54e7d8ff3a74 424
vsutardja 12:54e7d8ff3a74 425 while (true);
vsutardja 0:fcf070a88ba0 426 }