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

Committer:
vsutardja
Date:
Mon Apr 11 23:56:23 2016 +0000
Revision:
14:a0614f48e6ef
Parent:
13:d6e167698517
Child:
15:db95bb7c7f93
Race 1

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