EE192 Team 4 / Mbed 2 deprecated FixedPWM

Fixed PWM

Dependencies:   FastAnalogIn MODSERIAL PID QEI RPCInterface Servo mbed-rtos mbed telemetry

Fork of Sequential_Timing by EE192 Team 4

main.cpp@16:3ab3c4670f4f, 2016-04-12 (annotated)

Committer:
vsutardja
Date:
Tue Apr 12 02:03:50 2016 +0000
Revision:
16:3ab3c4670f4f
Parent:
15:db95bb7c7f93
Child:
17:bf6192a361ab
PID library for steering

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