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@17:bf6192a361ab, 2016-04-14 (annotated)

Committer:
vsutardja
Date:
Thu Apr 14 00:45:14 2016 +0000
Revision:
17:bf6192a361ab
Parent:
16:3ab3c4670f4f 
Current iter

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