EE192 Team 4 / Mbed 2 deprecated FixedPWMWill

Possibly faster steering response.

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

Fork of FixedPWMWill by Will Porter

main.cpp@28:4b76abe148cd, 2016-04-27 (annotated)

Committer:
vsutardja
Date:
Wed Apr 27 22:24:11 2016 +0000
Revision:
28:4b76abe148cd
Parent:
27:e796e9ee0495 
trying telemetry again

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 28:4b76abe148cd 10 #define set_clk() PTD->PSOR = (0x01);
vsutardja 28:4b76abe148cd 11 #define clr_clk() PTD->PCOR = (0x01);
vsutardja 28:4b76abe148cd 12 #define set_si() PTD->PSOR = (0x01 << 5);
vsutardja 28:4b76abe148cd 13 #define clr_si() PTD->PCOR = (0x01 << 5);
vsutardja 28:4b76abe148cd 14
vsutardja 12:54e7d8ff3a74 15
vsutardja 0:fcf070a88ba0 16 // =========
vsutardja 0:fcf070a88ba0 17 // Telemetry
vsutardja 0:fcf070a88ba0 18 // =========
vsutardja 18:2b7db50fec4c 19 //MODSERIAL telemetry_serial(PTC4, PTC3); // TX, RX
vsutardja 18:2b7db50fec4c 20 MODSERIAL telemetry_serial(USBTX, USBRX);
vsutardja 12:54e7d8ff3a74 21 telemetry::MbedHal telemetry_hal(telemetry_serial); // Hardware Abstraction Layer
vsutardja 12:54e7d8ff3a74 22 telemetry::Telemetry telemetry_obj(telemetry_hal); // Telemetry
vsutardja 11:4348bba086a4 23
vsutardja 11:4348bba086a4 24 telemetry::Numeric<uint32_t> tele_time_ms(telemetry_obj, "time", "Time", "ms", 0);
vsutardja 27:e796e9ee0495 25 telemetry::NumericArray<uint16_t, 128> tele_linescan(telemetry_obj, "linescan", "Linescan", "ADC", 0);
vsutardja 28:4b76abe148cd 26 telemetry::Numeric<uint8_t> tele_center(telemetry_obj, "tele_center", "Center", "px", 0);
vsutardja 12:54e7d8ff3a74 27 telemetry::Numeric<float> tele_velocity(telemetry_obj, "tele_vel", "Velocity", "who knows", 0);
vsutardja 0:fcf070a88ba0 28
vsutardja 12:54e7d8ff3a74 29 Timer t_tele;
wretrop 19:296fb2f473a3 30 Timeout expo_time;
vsutardja 12:54e7d8ff3a74 31
wretrop 19:296fb2f473a3 32 float interrupt_T = 0.015; // Something is very wrong here.
vsutardja 0:fcf070a88ba0 33
vsutardja 0:fcf070a88ba0 34 // =============
vsutardja 0:fcf070a88ba0 35 // Communication
vsutardja 0:fcf070a88ba0 36 // =============
vsutardja 12:54e7d8ff3a74 37 char comm_cmd; // Command
vsutardja 18:2b7db50fec4c 38 char comm_in[8]; // Input
vsutardja 18:2b7db50fec4c 39 //Serial bt(USBTX, USBRX); // USB connection
vsutardja 18:2b7db50fec4c 40 Serial bt(PTC4, PTC3); // BlueSMiRF connection
vsutardja 0:fcf070a88ba0 41
vsutardja 12:54e7d8ff3a74 42 void communication(void const *args); // Communications
vsutardja 0:fcf070a88ba0 43
vsutardja 0:fcf070a88ba0 44 // =====
vsutardja 0:fcf070a88ba0 45 // Motor
vsutardja 0:fcf070a88ba0 46 // =====
vsutardja 28:4b76abe148cd 47 const float motor_T = 1.0 / 100; // Frequency
vsutardja 12:54e7d8ff3a74 48 float motor_duty = 0.0; // Duty cycle
vsutardja 12:54e7d8ff3a74 49 bool e_stop = false; // Emergency stop
vsutardja 28:4b76abe148cd 50 InterruptIn bemf_int(PTD4); // Back EMF measurement trigger
vsutardja 24:d206fa06c610 51 PwmOut motor(PTD4); // Enable pin (PWM)
vsutardja 28:4b76abe148cd 52 Timeout bemf_timeout; // Back EMF interrupt delay
vsutardja 28:4b76abe148cd 53 FastAnalogIn bemf(PTB3, 0); // Back EMF read pin
vsutardja 28:4b76abe148cd 54 int bemf_vel = 0; // Back EMF reading
vsutardja 12:54e7d8ff3a74 55 float velocity = 0; // Velocity
vsutardja 28:4b76abe148cd 56 int motor_ctrl_count = 0; // Velocity control decimation counter
vsutardja 28:4b76abe148cd 57 float Kmp = 12.0; // Proportional factor
vsutardja 28:4b76abe148cd 58 float Kmi = 0; // Integral factor
vsutardja 28:4b76abe148cd 59 float Kmd = 0; // Derivative factor
vsutardja 28:4b76abe148cd 60 float master_v = 0; // Master velocity
vsutardja 28:4b76abe148cd 61 float ref_v = 0; // Reference velocity
vsutardja 28:4b76abe148cd 62 PID motor_ctrl(Kmp, Kmi, Kmd, interrupt_T); // Motor controller
vsutardja 28:4b76abe148cd 63 int turn_thresh = 19; // Minimum error to determine turn
vsutardja 28:4b76abe148cd 64 float turn_gain = 0.06; // Proportional gain for turning reference velocity
vsutardja 28:4b76abe148cd 65 float turn_minv = 1.0; // Minimum turning velocity
vsutardja 22:b9969eaf0e80 66
vsutardja 0:fcf070a88ba0 67 // =====
vsutardja 0:fcf070a88ba0 68 // Servo
vsutardja 0:fcf070a88ba0 69 // =====
vsutardja 12:54e7d8ff3a74 70 float angle = 88; // Angle
vsutardja 28:4b76abe148cd 71 float Ksp = 1.5; // Steering proportion
vsutardja 28:4b76abe148cd 72 float Ksi = 9000000; // Steering integral
vsutardja 28:4b76abe148cd 73 float Ksd = 1.2; // Steering derivative
vsutardja 28:4b76abe148cd 74 PID servo_ctrl(Ksp, Ksi, Ksd, interrupt_T); // Servo controller
vsutardja 12:54e7d8ff3a74 75 Servo servo(PTA12); // Signal pin (PWM)
vsutardja 0:fcf070a88ba0 76
vsutardja 0:fcf070a88ba0 77 // ======
vsutardja 0:fcf070a88ba0 78 // Camera
vsutardja 0:fcf070a88ba0 79 // ======
vsutardja 28:4b76abe148cd 80 int fixed_t_int = 1100; // Fixed exposure time
vsutardja 28:4b76abe148cd 81 int t_int = 10000; // Variable exposure time
vsutardja 28:4b76abe148cd 82 bool rdyFlag = 1; // Camera ready
vsutardja 28:4b76abe148cd 83 bool dataFlag = 0; // Data ready
vsutardja 18:2b7db50fec4c 84 const int T_INT_MAX = interrupt_T * 1000000 - 1000; // Maximum exposure time
vsutardja 12:54e7d8ff3a74 85 const int T_INT_MIN = 35; // Minimum exposure time
vsutardja 27:e796e9ee0495 86 int img[128] = {0}; // Image data
vsutardja 24:d206fa06c610 87 DigitalOut clk(PTD0); // Clock pin
vsutardja 24:d206fa06c610 88 DigitalOut si(PTD5); // SI pin
vsutardja 12:54e7d8ff3a74 89 FastAnalogIn ao(PTC2); // AO pin
vsutardja 0:fcf070a88ba0 90
vsutardja 0:fcf070a88ba0 91 // ================
vsutardja 0:fcf070a88ba0 92 // Image processing
vsutardja 0:fcf070a88ba0 93 // ================
vsutardja 12:54e7d8ff3a74 94 int max = -1; // Maximum luminosity
vsutardja 12:54e7d8ff3a74 95 int left[5] = {0}; // Left edge
vsutardja 12:54e7d8ff3a74 96 int right[5] = {0}; // Right edge
vsutardja 12:54e7d8ff3a74 97 int j = 0; // Peaks index
vsutardja 12:54e7d8ff3a74 98 int center = 64; // Center estimate
vsutardja 12:54e7d8ff3a74 99 int centers[5] = {0}; // Possible centers
vsutardja 12:54e7d8ff3a74 100 int prev_center = 64; // Previous center
vsutardja 12:54e7d8ff3a74 101 float scores[5] = {0}; // Candidate scores
vsutardja 12:54e7d8ff3a74 102 int best_score_idx = 0; // Most likely center index
vsutardja 0:fcf070a88ba0 103
vsutardja 0:fcf070a88ba0 104 // ================
vsutardja 0:fcf070a88ba0 105 // Functions
vsutardja 0:fcf070a88ba0 106 // ================
vsutardja 0:fcf070a88ba0 107
vsutardja 28:4b76abe148cd 108 void rxCallback(MODSERIAL_IRQ_INFO *q) {
vsutardja 28:4b76abe148cd 109 MODSERIAL *serial = q->serial;
vsutardja 28:4b76abe148cd 110 if ( serial->rxGetLastChar() == '+') {
vsutardja 28:4b76abe148cd 111 master_v = master_v + 0.25;
vsutardja 28:4b76abe148cd 112 motor_ctrl.setSetPoint(master_v);
vsutardja 28:4b76abe148cd 113 }
vsutardja 28:4b76abe148cd 114 if ( serial->rxGetLastChar() == '-') {
vsutardja 28:4b76abe148cd 115 master_v = master_v - 0.25;
vsutardja 28:4b76abe148cd 116 motor_ctrl.setSetPoint(master_v);
vsutardja 28:4b76abe148cd 117 }
vsutardja 28:4b76abe148cd 118 }
vsutardja 13:d6e167698517 119
vsutardja 12:54e7d8ff3a74 120 // Communications
vsutardja 14:a0614f48e6ef 121 void communication(void const *args) {
vsutardja 28:4b76abe148cd 122 telemetry_serial.baud(115200);
vsutardja 28:4b76abe148cd 123 telemetry_serial.attach(&rxCallback, MODSERIAL::RxIrq);
vsutardja 28:4b76abe148cd 124 telemetry_obj.transmit_header();
vsutardja 14:a0614f48e6ef 125 while (true) {
vsutardja 28:4b76abe148cd 126 tele_time_ms = t_tele.read_ms();
vsutardja 28:4b76abe148cd 127 telemetry_obj.do_io();
vsutardja 28:4b76abe148cd 128 }
vsutardja 28:4b76abe148cd 129 }
wretrop 19:296fb2f473a3 130
vsutardja 28:4b76abe148cd 131 //void communication(void const *args) {
vsutardja 28:4b76abe148cd 132 // // Initialize bluetooth
vsutardja 28:4b76abe148cd 133 // bt.baud(115200);
vsutardja 28:4b76abe148cd 134 //
vsutardja 28:4b76abe148cd 135 // while (true) {
vsutardja 28:4b76abe148cd 136 //
vsutardja 28:4b76abe148cd 137 // bt.printf("\r\nPress q to return to this prompt.\r\n");
vsutardja 28:4b76abe148cd 138 // bt.printf("Available diagnostics:\r\n");
vsutardja 28:4b76abe148cd 139 //// bt.printf(" [0] Velocity\r\n");
vsutardja 28:4b76abe148cd 140 // bt.printf(" [0] Change exposure (us)\r\n");
vsutardja 28:4b76abe148cd 141 // bt.printf(" [1] Steering\r\n");
vsutardja 28:4b76abe148cd 142 // bt.printf(" [2] Change Ksp\r\n");
vsutardja 28:4b76abe148cd 143 // bt.printf(" [3] Change Ksi\r\n");
vsutardja 28:4b76abe148cd 144 // bt.printf(" [4] Change Ksd\r\n");
vsutardja 28:4b76abe148cd 145 // bt.printf(" [5] Change turn slowing minimum velocity\r\n");
vsutardja 28:4b76abe148cd 146 // bt.printf(" [6] Change turn slowing gain\r\n");
vsutardja 28:4b76abe148cd 147 // bt.printf(" [7] Change turn slowing dead zone\r\n");
vsutardja 28:4b76abe148cd 148 // bt.printf(" [8] master_v += 0.05\r\n");
vsutardja 28:4b76abe148cd 149 // bt.printf(" [9] master_v -= 0.05\r\n");
vsutardja 28:4b76abe148cd 150 // comm_cmd = bt.getc();
vsutardja 28:4b76abe148cd 151 // while (comm_cmd != 'q') {
vsutardja 28:4b76abe148cd 152 // t.reset();
vsutardja 28:4b76abe148cd 153 // t.start();
vsutardja 28:4b76abe148cd 154 // switch(atoi(&comm_cmd)){
vsutardja 28:4b76abe148cd 155 // case 0:
vsutardja 28:4b76abe148cd 156 //// bt.printf("bemf: %d, Duty cycle: %f, Pulse count: %d, Velocity: %f, Kmp: %f, Kmi: %f, Kmd: %f\r\n", bemf_vel, motor_duty, curr_pulses, velocity, Kmp, Kmi, Kmd);
vsutardja 28:4b76abe148cd 157 // bt.printf("Current: %d, New (8 digits): ", fixed_t_int);
vsutardja 18:2b7db50fec4c 158 // for (int i = 0; i < 8; i++) {
vsutardja 18:2b7db50fec4c 159 // comm_in[i] = bt.getc();
vsutardja 18:2b7db50fec4c 160 // bt.putc(comm_in[i]);
vsutardja 18:2b7db50fec4c 161 // }
vsutardja 18:2b7db50fec4c 162 // bt.printf("\r\n");
vsutardja 28:4b76abe148cd 163 // fixed_t_int = atoi(comm_in);
vsutardja 18:2b7db50fec4c 164 // comm_cmd = 'q';
vsutardja 18:2b7db50fec4c 165 // break;
vsutardja 28:4b76abe148cd 166 // case 1:
vsutardja 28:4b76abe148cd 167 // bt.printf("max: %d, angle: %f, center: %d, t_int: %d, rdyFlag: %d, dataFlag: %d\r\n", max, angle, center, t_int, rdyFlag, dataFlag);
vsutardja 28:4b76abe148cd 168 // break;
vsutardja 28:4b76abe148cd 169 // case 2:
vsutardja 28:4b76abe148cd 170 // bt.printf("Current: %f, New (8 digits): ", Ksp);
vsutardja 18:2b7db50fec4c 171 // for (int i = 0; i < 8; i++) {
vsutardja 18:2b7db50fec4c 172 // comm_in[i] = bt.getc();
vsutardja 18:2b7db50fec4c 173 // bt.putc(comm_in[i]);
vsutardja 18:2b7db50fec4c 174 // }
vsutardja 18:2b7db50fec4c 175 // bt.printf("\r\n");
vsutardja 28:4b76abe148cd 176 // Ksp = atof(comm_in);
vsutardja 28:4b76abe148cd 177 // servo_ctrl.setTunings(Ksp, Ksi, Ksd);
vsutardja 28:4b76abe148cd 178 // servo_ctrl.reset();
vsutardja 18:2b7db50fec4c 179 // comm_cmd = 'q';
vsutardja 18:2b7db50fec4c 180 // break;
vsutardja 28:4b76abe148cd 181 // case 3:
vsutardja 28:4b76abe148cd 182 // bt.printf("Current: %f, New (8 digits): ", Ksi);
vsutardja 18:2b7db50fec4c 183 // for (int i = 0; i < 8; i++) {
vsutardja 18:2b7db50fec4c 184 // comm_in[i] = bt.getc();
vsutardja 18:2b7db50fec4c 185 // bt.putc(comm_in[i]);
vsutardja 18:2b7db50fec4c 186 // }
vsutardja 18:2b7db50fec4c 187 // bt.printf("\r\n");
vsutardja 28:4b76abe148cd 188 // Ksi = atof(comm_in);
vsutardja 28:4b76abe148cd 189 // servo_ctrl.setTunings(Ksp, Ksi, Ksd);
vsutardja 28:4b76abe148cd 190 // servo_ctrl.reset();
vsutardja 18:2b7db50fec4c 191 // comm_cmd = 'q';
vsutardja 18:2b7db50fec4c 192 // break;
vsutardja 28:4b76abe148cd 193 // case 4:
vsutardja 28:4b76abe148cd 194 // bt.printf("Current: %f, New (8 digits): ", Ksd);
vsutardja 22:b9969eaf0e80 195 // for (int i = 0; i < 8; i++) {
vsutardja 22:b9969eaf0e80 196 // comm_in[i] = bt.getc();
vsutardja 22:b9969eaf0e80 197 // bt.putc(comm_in[i]);
vsutardja 22:b9969eaf0e80 198 // }
vsutardja 22:b9969eaf0e80 199 // bt.printf("\r\n");
vsutardja 28:4b76abe148cd 200 // Ksd = atof(comm_in);
vsutardja 28:4b76abe148cd 201 // servo_ctrl.setTunings(Ksp, Ksi, Ksd);
vsutardja 28:4b76abe148cd 202 // servo_ctrl.reset();
vsutardja 22:b9969eaf0e80 203 // comm_cmd = 'q';
vsutardja 22:b9969eaf0e80 204 // break;
vsutardja 28:4b76abe148cd 205 // case 5:
vsutardja 28:4b76abe148cd 206 // bt.printf("Current: %f, New (8 digits): ", turn_minv);
vsutardja 28:4b76abe148cd 207 // for (int i = 0; i < 8; i++) {
vsutardja 28:4b76abe148cd 208 // comm_in[i] = bt.getc();
vsutardja 28:4b76abe148cd 209 // bt.putc(comm_in[i]);
vsutardja 28:4b76abe148cd 210 // }
vsutardja 28:4b76abe148cd 211 // bt.printf("\r\n");
vsutardja 28:4b76abe148cd 212 // turn_minv = atof(comm_in);
vsutardja 28:4b76abe148cd 213 // comm_cmd = 'q';
vsutardja 28:4b76abe148cd 214 // break;
wretrop 20:8f4838b0d94d 215 // case 6:
vsutardja 28:4b76abe148cd 216 // bt.printf("Current: %f, New (8 digits): ", turn_gain);
vsutardja 28:4b76abe148cd 217 // for (int i = 0; i < 8; i++) {
vsutardja 28:4b76abe148cd 218 // comm_in[i] = bt.getc();
vsutardja 28:4b76abe148cd 219 // bt.putc(comm_in[i]);
vsutardja 28:4b76abe148cd 220 // }
vsutardja 28:4b76abe148cd 221 // bt.printf("\r\n");
vsutardja 28:4b76abe148cd 222 // turn_gain = atof(comm_in);
vsutardja 28:4b76abe148cd 223 // comm_cmd = 'q';
vsutardja 28:4b76abe148cd 224 // break;
vsutardja 28:4b76abe148cd 225 // case 7:
vsutardja 28:4b76abe148cd 226 // bt.printf("Current: %d, New (8 digits): ", turn_thresh);
wretrop 20:8f4838b0d94d 227 // for (int i = 0; i < 8; i++) {
wretrop 20:8f4838b0d94d 228 // comm_in[i] = bt.getc();
wretrop 20:8f4838b0d94d 229 // bt.putc(comm_in[i]);
wretrop 20:8f4838b0d94d 230 // }
wretrop 20:8f4838b0d94d 231 // bt.printf("\r\n");
vsutardja 28:4b76abe148cd 232 // turn_thresh = atoi(comm_in);
wretrop 20:8f4838b0d94d 233 // comm_cmd = 'q';
wretrop 20:8f4838b0d94d 234 // break;
vsutardja 28:4b76abe148cd 235 // case 8:
vsutardja 28:4b76abe148cd 236 // master_v = master_v + 0.25;
vsutardja 28:4b76abe148cd 237 // motor_ctrl.setSetPoint(master_v);
vsutardja 28:4b76abe148cd 238 // bt.printf("%f\r\n", master_v);
vsutardja 22:b9969eaf0e80 239 // comm_cmd = 'q';
vsutardja 22:b9969eaf0e80 240 // break;
vsutardja 28:4b76abe148cd 241 // case 9:
vsutardja 28:4b76abe148cd 242 // master_v = master_v - 0.25;
vsutardja 28:4b76abe148cd 243 // motor_ctrl.setSetPoint(master_v);
vsutardja 28:4b76abe148cd 244 // bt.printf("%f\r\n", master_v);
vsutardja 28:4b76abe148cd 245 // comm_cmd = 'q';
vsutardja 18:2b7db50fec4c 246 // break;
vsutardja 28:4b76abe148cd 247 // }
vsutardja 28:4b76abe148cd 248 // if (bt.readable()) {
vsutardja 28:4b76abe148cd 249 // comm_cmd = bt.getc();
vsutardja 28:4b76abe148cd 250 // }
vsutardja 28:4b76abe148cd 251 // }
vsutardja 28:4b76abe148cd 252 //
vsutardja 28:4b76abe148cd 253 // }
vsutardja 27:e796e9ee0495 254 //}
vsutardja 27:e796e9ee0495 255
wretrop 19:296fb2f473a3 256 void cam_Read(){
vsutardja 28:4b76abe148cd 257 // Image capture
vsutardja 28:4b76abe148cd 258 clr_clk();
vsutardja 28:4b76abe148cd 259 set_si();
vsutardja 28:4b76abe148cd 260 set_clk();
vsutardja 28:4b76abe148cd 261 clr_si();
vsutardja 0:fcf070a88ba0 262
vsutardja 0:fcf070a88ba0 263 for (int i = 0; i < 128; i++) {
vsutardja 28:4b76abe148cd 264 clr_clk();
vsutardja 27:e796e9ee0495 265 img[i] = ao.read_u16();
vsutardja 28:4b76abe148cd 266 tele_linescan[i] = img[i];
vsutardja 28:4b76abe148cd 267 set_clk();
vsutardja 0:fcf070a88ba0 268 }
wretrop 19:296fb2f473a3 269 dataFlag = 1;
wretrop 19:296fb2f473a3 270 }
wretrop 19:296fb2f473a3 271
wretrop 19:296fb2f473a3 272 void control() {
wretrop 19:296fb2f473a3 273 if (rdyFlag){
vsutardja 28:4b76abe148cd 274 // Dummy read
vsutardja 28:4b76abe148cd 275 clr_clk();
vsutardja 28:4b76abe148cd 276 set_si();
vsutardja 28:4b76abe148cd 277 set_clk();
vsutardja 28:4b76abe148cd 278 clr_clk();
vsutardja 0:fcf070a88ba0 279
wretrop 19:296fb2f473a3 280 for (int i = 0; i < 128; i++) {
vsutardja 28:4b76abe148cd 281 clr_clk();
vsutardja 28:4b76abe148cd 282 set_clk();
wretrop 19:296fb2f473a3 283 }
wretrop 19:296fb2f473a3 284
wretrop 19:296fb2f473a3 285 // Expose
wretrop 19:296fb2f473a3 286 expo_time.attach_us(&cam_Read,t_int);
wretrop 19:296fb2f473a3 287 rdyFlag = 0;
wretrop 19:296fb2f473a3 288 }
wretrop 19:296fb2f473a3 289
vsutardja 12:54e7d8ff3a74 290 // Detect peak edges
wretrop 19:296fb2f473a3 291 if(dataFlag){
wretrop 19:296fb2f473a3 292 j = 0;
vsutardja 27:e796e9ee0495 293 for (int i = 0; i < 127 && j < 5; i++) {
wretrop 26:722362964784 294 if (img[i] > max * 0.66) {
wretrop 19:296fb2f473a3 295 left[j] = i;
wretrop 26:722362964784 296 while (img[i] > max * 0.66) {
wretrop 19:296fb2f473a3 297 i = i + 1;
wretrop 19:296fb2f473a3 298 }
wretrop 19:296fb2f473a3 299 right[j] = i;
wretrop 19:296fb2f473a3 300 j = j + 1;
vsutardja 0:fcf070a88ba0 301 }
wretrop 19:296fb2f473a3 302 }
wretrop 19:296fb2f473a3 303
wretrop 19:296fb2f473a3 304 // Calculate peak centers
wretrop 19:296fb2f473a3 305 for (int i = 0; i < j; i++) {
vsutardja 27:e796e9ee0495 306 centers[i] = (left[i] + right[i]) / 2;
wretrop 19:296fb2f473a3 307 }
wretrop 19:296fb2f473a3 308
wretrop 19:296fb2f473a3 309 // Assign scores
wretrop 19:296fb2f473a3 310 for (int i = 0; i < j; i++) {
wretrop 19:296fb2f473a3 311 scores[i] = 10 / (right[i] - left[i]) + img[centers[i]] / 65536 + 10 / abs(centers[i] - prev_center);
wretrop 19:296fb2f473a3 312 }
wretrop 19:296fb2f473a3 313
wretrop 19:296fb2f473a3 314 // Choose most likely center
wretrop 19:296fb2f473a3 315 best_score_idx = 0;
wretrop 19:296fb2f473a3 316 for (int i = 0; i < j; i++) {
wretrop 19:296fb2f473a3 317 if (scores[i] > scores[best_score_idx]) {
wretrop 19:296fb2f473a3 318 best_score_idx = i;
wretrop 19:296fb2f473a3 319 }
vsutardja 0:fcf070a88ba0 320 }
wretrop 19:296fb2f473a3 321 prev_center = center;
wretrop 19:296fb2f473a3 322 center = centers[best_score_idx];
vsutardja 28:4b76abe148cd 323 tele_center = center;
wretrop 19:296fb2f473a3 324
vsutardja 28:4b76abe148cd 325 servo_ctrl.setProcessValue(center);
vsutardja 28:4b76abe148cd 326 angle = 88 + servo_ctrl.compute();
wretrop 19:296fb2f473a3 327 servo = angle / 180;
wretrop 19:296fb2f473a3 328
vsutardja 22:b9969eaf0e80 329
wretrop 19:296fb2f473a3 330 // AGC
wretrop 19:296fb2f473a3 331 max = -1;
vsutardja 27:e796e9ee0495 332 for (int i = 0; i < 128; i++) {
wretrop 19:296fb2f473a3 333 if (img[i] > max) {
wretrop 19:296fb2f473a3 334 max = img[i];
wretrop 19:296fb2f473a3 335 }
vsutardja 12:54e7d8ff3a74 336 }
wretrop 19:296fb2f473a3 337 if (max > 60000) {
wretrop 19:296fb2f473a3 338 t_int = t_int - 0.1 * (max - 60000);
wretrop 19:296fb2f473a3 339 }
vsutardja 27:e796e9ee0495 340 if (max < 40000) {
vsutardja 27:e796e9ee0495 341 t_int = t_int + 0.1 * (40000 - max);
wretrop 19:296fb2f473a3 342 }
wretrop 19:296fb2f473a3 343 if (t_int < T_INT_MIN) {
wretrop 19:296fb2f473a3 344 t_int = T_INT_MIN;
wretrop 19:296fb2f473a3 345 }
wretrop 19:296fb2f473a3 346 if (t_int > T_INT_MAX) {
wretrop 19:296fb2f473a3 347 t_int = T_INT_MAX;
wretrop 19:296fb2f473a3 348 }
vsutardja 27:e796e9ee0495 349 t_int = fixed_t_int;
wretrop 20:8f4838b0d94d 350 servo_ctrl.setInterval(t_int);
wretrop 19:296fb2f473a3 351 rdyFlag = 1;
wretrop 19:296fb2f473a3 352 dataFlag = 0;
vsutardja 12:54e7d8ff3a74 353 }
vsutardja 22:b9969eaf0e80 354
vsutardja 28:4b76abe148cd 355 // Velocity control
vsutardja 22:b9969eaf0e80 356 if (motor_ctrl_count == 1000) {
vsutardja 24:d206fa06c610 357 velocity = (40000 - bemf_vel) / 6000.0;
vsutardja 28:4b76abe148cd 358 tele_velocity = velocity;
vsutardja 22:b9969eaf0e80 359 motor_ctrl_count = 0;
vsutardja 22:b9969eaf0e80 360 motor_ctrl.setProcessValue(velocity);
vsutardja 22:b9969eaf0e80 361 motor_duty = motor_ctrl.compute();
vsutardja 24:d206fa06c610 362 motor = motor_duty;
vsutardja 22:b9969eaf0e80 363 motor_ctrl_count = 0;
vsutardja 22:b9969eaf0e80 364 } else {
vsutardja 22:b9969eaf0e80 365 motor_ctrl_count = motor_ctrl_count + 1;
vsutardja 22:b9969eaf0e80 366 }
vsutardja 22:b9969eaf0e80 367
vsutardja 28:4b76abe148cd 368 // Turn handling
vsutardja 24:d206fa06c610 369 ref_v = master_v - master_v * turn_gain * (abs(64 - center) - turn_thresh);
vsutardja 24:d206fa06c610 370 if (ref_v < turn_minv) {
vsutardja 24:d206fa06c610 371 ref_v = turn_minv;
vsutardja 24:d206fa06c610 372 }
vsutardja 24:d206fa06c610 373 if (ref_v > master_v) {
vsutardja 24:d206fa06c610 374 ref_v = master_v;
vsutardja 24:d206fa06c610 375 }
vsutardja 24:d206fa06c610 376 if (abs(motor_ctrl.getSetPoint() - ref_v) > 0.05) {
vsutardja 24:d206fa06c610 377 motor_ctrl.setSetPoint(ref_v);
vsutardja 24:d206fa06c610 378 }
vsutardja 14:a0614f48e6ef 379 }
vsutardja 14:a0614f48e6ef 380
vsutardja 28:4b76abe148cd 381 // Back emf measuring interrupt
vsutardja 22:b9969eaf0e80 382 void meas_bemf() {
vsutardja 22:b9969eaf0e80 383 bemf_vel = bemf.read_u16();
vsutardja 22:b9969eaf0e80 384 }
vsutardja 22:b9969eaf0e80 385
vsutardja 28:4b76abe148cd 386 // Back emf trigger
vsutardja 22:b9969eaf0e80 387 void bemf_irq() {
vsutardja 22:b9969eaf0e80 388 bemf_timeout.attach(&meas_bemf, 0.002);
vsutardja 22:b9969eaf0e80 389 }
vsutardja 22:b9969eaf0e80 390
vsutardja 0:fcf070a88ba0 391 // ====
vsutardja 0:fcf070a88ba0 392 // Main
vsutardja 0:fcf070a88ba0 393 // ====
vsutardja 0:fcf070a88ba0 394 int main() {
vsutardja 28:4b76abe148cd 395 t_tele.start();
vsutardja 18:2b7db50fec4c 396
vsutardja 0:fcf070a88ba0 397 // Initialize motor
vsutardja 22:b9969eaf0e80 398 motor.period(motor_T);
vsutardja 24:d206fa06c610 399 motor = motor_duty;
vsutardja 0:fcf070a88ba0 400
vsutardja 0:fcf070a88ba0 401 // Initialize motor controller
vsutardja 0:fcf070a88ba0 402 motor_ctrl.setInputLimits(0.0, 10.0);
vsutardja 22:b9969eaf0e80 403 motor_ctrl.setOutputLimits(0.05, 0.75);
vsutardja 24:d206fa06c610 404 motor_ctrl.setSetPoint(master_v);
vsutardja 0:fcf070a88ba0 405 motor_ctrl.setBias(0.0);
vsutardja 0:fcf070a88ba0 406 motor_ctrl.setMode(1);
vsutardja 0:fcf070a88ba0 407
vsutardja 28:4b76abe148cd 408 // Initialize back EMF reader
vsutardja 28:4b76abe148cd 409 bemf_int.fall(&bemf_irq);
vsutardja 28:4b76abe148cd 410
vsutardja 12:54e7d8ff3a74 411 // Initialize servo
vsutardja 12:54e7d8ff3a74 412 servo.calibrate(0.001, 45.0);
vsutardja 12:54e7d8ff3a74 413 servo = angle / 180.0;
vsutardja 0:fcf070a88ba0 414
vsutardja 28:4b76abe148cd 415 // Initialize servo controller
vsutardja 27:e796e9ee0495 416 servo_ctrl.setInputLimits(0, 127);
wretrop 19:296fb2f473a3 417 servo_ctrl.setOutputLimits(-30, 30);
vsutardja 27:e796e9ee0495 418 servo_ctrl.setSetPoint(64);
vsutardja 18:2b7db50fec4c 419 servo_ctrl.setBias(0.0);
vsutardja 16:3ab3c4670f4f 420 servo_ctrl.setMode(1);
vsutardja 16:3ab3c4670f4f 421
vsutardja 0:fcf070a88ba0 422 // Initialize communications thread
vsutardja 18:2b7db50fec4c 423 Thread communication_thread(communication);
vsutardja 12:54e7d8ff3a74 424
vsutardja 28:4b76abe148cd 425 // Run
vsutardja 14:a0614f48e6ef 426 while (true) {
vsutardja 28:4b76abe148cd 427 control();
vsutardja 28:4b76abe148cd 428 }
vsutardja 0:fcf070a88ba0 429 }