Possibly faster steering response.
Dependencies: FastAnalogIn MODSERIAL PID QEI RPCInterface Servo mbed-rtos mbed telemetry
Fork of FixedPWMWill by
Diff: main.cpp
- Revision:
- 12:54e7d8ff3a74
- Parent:
- 11:4348bba086a4
- Child:
- 13:d6e167698517
--- a/main.cpp Tue Apr 05 21:52:32 2016 +0000 +++ b/main.cpp Fri Apr 08 21:40:35 2016 +0000 @@ -1,120 +1,123 @@ +#include "FastAnalogIn.h" #include "mbed.h" -#include "rtos.h" -#include "Servo.h" -#include "FastAnalogIn.h" #include "PID.h" #include "QEI.h" +#include "rtos.h" +#include "SerialRPCInterface.h" +#include "Servo.h" #include "telemetry.h" + // ========= // Telemetry // ========= -MODSERIAL telemetry_serial(PTC4, PTC3); // TX, RX -telemetry::MbedHal telemetry_hal(telemetry_serial); // Hardware Abstraction Layer -telemetry::Telemetry telemetry_obj(telemetry_hal); // Telemetry +MODSERIAL telemetry_serial(PTC4, PTC3); // TX, RX +//MODSERIAL telemetry_serial(USBTX, USBRX); +telemetry::MbedHal telemetry_hal(telemetry_serial); // Hardware Abstraction Layer +telemetry::Telemetry telemetry_obj(telemetry_hal); // Telemetry -int dec = 0; -Timer t; telemetry::Numeric<uint32_t> tele_time_ms(telemetry_obj, "time", "Time", "ms", 0); -telemetry::NumericArray<uint16_t, 128> tele_linescan(telemetry_obj, "linescan", "Linescan", "ADC", 0); -//telemetry::Numeric<float> tele_vel(telemetry_obj, "t_vel", "Velocity", "m/s", 0); -telemetry::Numeric<uint32_t> tele_center(telemetry_obj, "t_center", "Center", "", 0); -telemetry::Numeric<uint32_t> tele_t_int(telemetry_obj, "t_t_int", "t_int", "us", 0); -telemetry::Numeric<uint32_t> tele_cam_dec(telemetry_obj, "t_cam_dec", "decimation", "", 0); +telemetry::NumericArray<uint16_t, 108> tele_linescan(telemetry_obj, "linescan", "Linescan", "ADC", 0); +telemetry::Numeric<uint32_t> tele_exposure(telemetry_obj, "exposure", "Exposure", "us", 0); +telemetry::Numeric<float> tele_velocity(telemetry_obj, "tele_vel", "Velocity", "who knows", 0); +telemetry::Numeric<float> tele_pwm(telemetry_obj, "pwm", "PWM", "", 0); +telemetry::Numeric<uint8_t> tele_center(telemetry_obj, "tele_center", "Center", "px", 0); +Timer t; +Timer t_tele; +Ticker control_interrupt; +int t_cam = 0; +int t_steer = 0; +int t_vel = 0; + +float interrupt_T = 0.025; // ============= // Communication // ============= -Serial pc(USBTX, USBRX); // USB connection -//Serial bt(PTC4, PTC3); // BlueSMiRF connection -char cmd; // Command -char ch; -char in[5]; +char comm_cmd; // Command +char comm_in[5]; // Input +//Serial pc(USBTX, USBRX); // USB connection +//Serial bt(PTC4, PTC3); // BlueSMiRF connection -void communication(void const *args); // Communications +void communication(void const *args); // Communications // ===== // Motor // ===== -PwmOut motor(PTA4); // Enable pin (PWM) -int T = 25000; // Frequency -float d = 0.0; // Duty cycle +const int motor_T = 25000; // Frequency +float motor_duty = 0.0; // Duty cycle +bool e_stop = false; // Emergency stop +PwmOut motor(PTA4); // Enable pin (PWM) // ======= // Encoder // ======= -const int MVG_AVG = 4; -int ppr = 389; // Pulses per revolution -QEI qei(PTD3, PTD2, NC, ppr, QEI::X4_ENCODING); // Quadrature encoder -float c = 0.20106; // Wheel circumference -int prev_pulses = 0; // Previous pulse count -int curr_pulses = 0; // Current pulse count -float velocity = 0; // Velocity -float vel = 0; -float v_prev[MVG_AVG] = {0}; +const int ppr = 389; // Pulses per revolution +const float c = 0.20106; // Wheel circumference +int prev_pulses = 0; // Previous pulse count +int curr_pulses = 0; // Current pulse count +float velocity = 0; // Velocity +QEI qei(PTD3, PTD2, NC, ppr, QEI::X4_ENCODING); // Quadrature encoder // ======== // Velocity // ======== -float Kp = 6.0; // Proportional factor -float Ki = 0; // Integral factor -float Kd = 0; // Derivative factor -float interval = 0.1; // Sampling interval -float ref_v = 1.0; -PID motor_ctrl(Kp, Ki, Kd, interval); // Motor controller +float Kp = 6.0; // Proportional factor +float Ki = 0; // Integral factor +float Kd = 0; // Derivative factor +float interval = 0.01; // Sampling interval +float ref_v = 0.8; // Reference velocity +PID motor_ctrl(Kp, Ki, Kd, interval); // Motor controller // ===== // Servo // ===== -Servo servo(PTA12); // Enable pin (PWM) -float a = 88; // Angle -float Ks = 0.9; +float angle = 88; // Angle +float Ks = 0.9; // Steering proportion +Servo servo(PTA12); // Signal pin (PWM) // ====== // Camera // ====== -DigitalOut clk(PTD5); // Clock pin -DigitalOut si(PTD0); // SI pin -FastAnalogIn ao(PTC2); // AO pin -Timeout camera_read; // Camera read timeout -const int T_INT_MIN = 2500; -int cam_dec = 0; -int cam_dec_count = 0; -int t_int = 15000; // Exposure time -int img[128]; // Image data - -void readCamera(); // Read data from camera +int t_int = 10000; // Exposure time +const int T_INT_MAX = interrupt_T * 1000000 - 10000; // Maximum exposure time +const int T_INT_MIN = 35; // Minimum exposure time +int img[108] = {0}; // Image data +DigitalOut clk(PTD5); // Clock pin +DigitalOut si(PTD0); // SI pin +FastAnalogIn ao(PTC2); // AO pin // ================ // Image processing // ================ -int max = -1; -float lum_bg = 0; -float contrast = 0; -int argmax = 0; -int argmin = 0; -int temp[128]; -int center = 64; - -void track(); // Line-tracking steering +int max = -1; // Maximum luminosity +int left[5] = {0}; // Left edge +int right[5] = {0}; // Right edge +int j = 0; // Peaks index +int center = 64; // Center estimate +int centers[5] = {0}; // Possible centers +int prev_center = 64; // Previous center +float scores[5] = {0}; // Candidate scores +int best_score_idx = 0; // Most likely center index // ================ // Functions // ================ -void tele_comm(void const *args) { +// Communications +void communication(void const *args) { telemetry_serial.baud(115200); telemetry_obj.transmit_header(); while (true) { - tele_time_ms = t.read_ms(); + tele_time_ms = t_tele.read_ms(); telemetry_obj.do_io(); - Thread::wait(100); } } - -// Communications //void communication(void const *args) { +// // Initialize bluetooth +// bt.baud(115200); +// // while (true) { // bt.printf("\r\nPress q to return to this prompt.\r\n"); // bt.printf("Available diagnostics:\r\n"); @@ -125,100 +128,112 @@ // bt.printf(" [4] Change Kd\r\n"); // bt.printf(" [5] Change Ks\r\n"); // bt.printf(" [6] Change reference velocity\r\n"); -// cmd = bt.getc(); -// while (cmd != 'q') { -// switch(atoi(&cmd)){ +// bt.printf(" [7] EMERGENCY STOP\r\n"); +// bt.printf(" [8] Timing\r\n"); +// comm_cmd = bt.getc(); +// while (comm_cmd != 'q') { +// switch(atoi(&comm_cmd)){ // case 0: -// bt.printf("Duty cycle: %f, Pulse count: %d, Velocity: %f, Kp: %f, Ki: %f, Kd: %f\r\n", d, curr_pulses, velocity, Kp, Ki, Kd); +// 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); // break; // case 1: -// bt.printf("Servo angle: %f, Track center: %d, t_int: %d\r\n", a, center, t_int); +// bt.printf("Servo angle: %f, Track center: %d, t_int: %d\r\n", angle, center, t_int); // break; // case 2: // bt.printf("Current: %f, New (5 digits): ", Kp); // for (int i = 0; i < 5; i++) { -// in[i] = bt.getc(); -// bt.putc(in[i]); +// comm_in[i] = bt.getc(); +// bt.putc(comm_in[i]); // } // bt.printf("\r\n"); -// Kp = atof(in); +// Kp = atof(comm_in); // motor_ctrl.setTunings(Kp, Ki, Kd); -// cmd = 'q'; +// comm_cmd = 'q'; // break; // case 3: // bt.printf("Current: %f, New (5 digits): ", Ki); // for (int i = 0; i < 5; i++) { -// in[i] = bt.getc(); -// bt.putc(in[i]); +// comm_in[i] = bt.getc(); +// bt.putc(comm_in[i]); // } // bt.printf("\r\n"); -// Ki = atof(in); +// Ki = atof(comm_in); // motor_ctrl.setTunings(Kp, Ki, Kd); -// cmd = 'q'; +// comm_cmd = 'q'; // break; // case 4: // bt.printf("Current: %f, New (5 digits): ", Kd); // for (int i = 0; i < 5; i++) { -// in[i] = bt.getc(); -// bt.putc(in[i]); +// comm_in[i] = bt.getc(); +// bt.putc(comm_in[i]); // } // bt.printf("\r\n"); -// Kd = atof(in); +// Kd = atof(comm_in); // motor_ctrl.setTunings(Kp, Ki, Kd); -// cmd = 'q'; +// comm_cmd = 'q'; // break; // case 5: // bt.printf("Current: %f, New (5 digits): ", Ks); // for (int i = 0; i < 5; i++) { -// in[i] = bt.getc(); -// bt.putc(in[i]); +// comm_in[i] = bt.getc(); +// bt.putc(comm_in[i]); // } // bt.printf("\r\n"); -// Ks = atof(in); -// cmd = 'q'; +// Ks = atof(comm_in); +// comm_cmd = 'q'; // break; // case 6: // bt.printf("Current: %f, New (5 digits): ", ref_v); // for (int i = 0; i < 5; i++) { -// in[i] = bt.getc(); -// bt.putc(in[i]); +// comm_in[i] = bt.getc(); +// bt.putc(comm_in[i]); // } // bt.printf("\r\n"); -// ref_v = atof(in); +// ref_v = atof(comm_in); // motor_ctrl.setSetPoint(ref_v); -// cmd = 'q'; +// comm_cmd = 'q'; +// break; +// case 7: +// e_stop = true; +// bt.printf("STOPPED\r\n"); +// break; +// case 8: +// 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); // break; // } // if (bt.readable()) { -// cmd = bt.getc(); +// comm_cmd = bt.getc(); // } // } // } //} -// Read data from camera -void read_camera() { - //if (cam_dec_count < cam_dec) { -// si = 1; -// wait_us(1); -// clk = 1; -// wait_us(1); -// si = 0; -// -// for (int i = 0; i < 128; i++) { -// wait_us(1); -// clk = 0; -// wait_us(1); -// clk = 1; -// } -// clk = 0; -// cam_dec_count = cam_dec_count + 1; -// camera_read.attach_us(&read_camera, t_int); -// return; -// } - cam_dec_count = 0; +void control() { + // Image capture + t.reset(); + + // Dummy read + clk = 0; + wait_us(1); + si = 1; + wait_us(1); + clk = 1; + wait_us(1); + si = 0; - // Start data transfer + for (int i = 0; i < 128; i++) { + wait_us(1); + clk = 0; + wait_us(1); + clk = 1; + } + + // Expose + wait_us(t_int); + + // Read camera + clk = 0; + wait_us(1); si = 1; wait_us(1); clk = 1; @@ -226,129 +241,119 @@ si = 0; wait_us(1); - // Read camera data for (int i = 0; i < 128; i++) { clk = 0; - img[i] = ao.read_u16(); - tele_linescan[i] = img[i]; + if (i > 9 && i < 118) { + img[i-10] = ao.read_u16(); + tele_linescan[i-10] = img[i-10]; + } else { + wait_us(1); + } clk = 1; wait_us(1); } - clk = 0; + + t_cam = t.read_us(); - // Update servo angle - track(); + // Steering control + t.reset(); - // Set next frame exposure time - // camera_read.attach_us(&read_camera, t_int); -} - -// Find line center from image -// Take two-point moving average to smooth the data -// Find indices where max and min of smoothed data are attained -// Calculate and return midpoint of argmax and argmin -void track() { - max = -1; - lum_bg = 0; - argmax = 0; - argmin = 0; - for (int i = 0; i < 107; i++) { - if (img[i+11] > max) { - max = img[i+11]; - } - if (i == 126) { - temp[i-1] = (img[i+11] + img[i+1+11]) / 2 - temp[i-1]; - if (temp[i-1] > temp[argmax]) { - argmax = i - 1; + // Detect peak edges + j = 0; + for (int i = 0; i < 107 && j < 5; i++) { + if (img[i] > 45000) { + left[j] = i; + while (img[i] > 45000) { + i = i + 1; } - if (temp[i-1] < temp[argmin]) { - argmin = i - 1; - } - } else { - temp[i] = (img[i+11] + img[i+1+11]) / 2; - if (i > 0) { - temp[i-1] = temp[i] - temp[i-1]; - if (temp[i-1] > temp[argmax]) { - argmax = i - 1; - } - if (temp[i-1] < temp[argmin]) { - argmin = i - 1; - } - } + right[j] = i; + j = j + 1; } } - for (int i = 0; i < 10; i++) { - lum_bg = lum_bg + img[55 - 4 - i] / 20.0 + img[55 + 4 + i] / 20.0; + // Calculate peak centers + for (int i = 0; i < j; i++) { + centers[i] = (left[i] + right[i] + 10) / 2; + } + + // Assign scores + for (int i = 0; i < j; i++) { + scores[i] = 10 / (right[i] - left[i]) + img[centers[i]] / 65536 + 10 / abs(centers[i] - prev_center); } - contrast = (max - lum_bg) / lum_bg; + // Choose most likely center + best_score_idx = 0; + for (int i = 0; i < j; i++) { + if (scores[i] > scores[best_score_idx]) { + best_score_idx = i; + } + } + prev_center = center; + center = centers[best_score_idx]; + tele_center = center; -// if (contrast < 1.5) { - // Underexposed - //if (max < 60000) { -// t_int = t_int + 0.15 * (60000 - max); -// } -// // Overexposed -// if (lum_bg > 25000) { -// t_int = t_int - 0.15 * (lum_bg - 25000); -// } -// } - + // Set servo angle + angle = 88 + (55 - center) * Ks; + if (angle > 113) { + angle = 113; + } + if (angle < 63) { + angle = 63; + } + servo = angle / 180; + + // AGC + max = -1; + for (int i = 0; i < 107; i++) { + if (img[i] > max) { + max = img[i]; + } + } if (max > 60000) { t_int = t_int - 0.1 * (max - 60000); } if (max < 50000) { t_int = t_int + 0.1 * (50000 - max); } - - if (t_int < 1000) { - t_int = 1000; + if (t_int < T_INT_MIN) { + t_int = T_INT_MIN; + } + if (t_int > T_INT_MAX) { + t_int = T_INT_MAX; } - - tele_t_int = t_int; + tele_exposure = t_int; - //if (t_int < T_INT_MIN) { -// cam_dec = T_INT_MIN / t_int; -// } + t_steer = t.read_us(); - tele_cam_dec = cam_dec; + // wait_us(8000 - t.read_us()); - if (max > 43253 && argmax < argmin) { - center = (argmax + argmin + 2 + 11) / 2; - tele_center = center; - a = 88 + (55 - center) * Ks; - if (a > 113) { - a = 113; - } - if (a < 63) { - a = 63; - } - servo = a / 180; + // Velocity control + t.reset(); + if (!e_stop) { + curr_pulses = qei.getPulses(); + velocity = (curr_pulses - prev_pulses) / interrupt_T / ppr * c; + tele_velocity = velocity; + prev_pulses = curr_pulses; + motor_ctrl.setProcessValue(velocity); + motor_duty = motor_ctrl.compute(); + motor = 1.0 - motor_duty; + tele_pwm = motor_duty; } - -// camera_read.attach_us(&read_camera, 1000); - camera_read.attach_us(&read_camera, t_int); + t_vel = t.read_us(); } // ==== // Main // ==== int main() { -// osThreadSetPriority(osThreadGetId(), osPriorityRealTime); t.start(); + t_tele.start(); + tele_center.set_limits(0, 128); + tele_pwm.set_limits(0.0, 1.0); // Initialize motor - motor.period_us(T); - motor = 1.0 - d; - - // Initialize servo - servo.calibrate(0.001, 45.0); - servo = a / 180.0; - - // Initialize & start camera - clk = 0; - read_camera(); + motor.period_us(motor_T); + motor = 1.0 - motor_duty; // Initialize motor controller motor_ctrl.setInputLimits(0.0, 10.0); @@ -357,42 +362,14 @@ motor_ctrl.setBias(0.0); motor_ctrl.setMode(1); - // Initialize bluetooth -// bt.baud(115200); - -// osThreadSetPriority(osThreadGetId(), osPriorityAboveNormal); + // Initialize servo + servo.calibrate(0.001, 45.0); + servo = angle / 180.0; // Initialize communications thread -// Thread communication_thread(communication); - Thread tele_comm_thread(tele_comm); -// tele_comm_thread.set_priority(osPriorityBelowNormal); + Thread communication_thread(communication); - // Start motor controller - while (true) { -// telemetry_serial.printf("%d\r\n", t.read_ms()); -// if (dec == 100) { - //tele_time_ms = t.read_ms(); -// telemetry_obj.do_io(); -// dec = 0; -// } -// dec = dec + 1; - curr_pulses = qei.getPulses(); - //for (int i = 0; i < MVG_AVG - 1; i++) { -// v_prev[i] = abs(1.5 - 0.5 * (1.5 - v_prev[i+1])); -// } -// v_prev[MVG_AVG-1] = velocity; - velocity = (curr_pulses - prev_pulses)/ interval / ppr * c / 2.5; -// tele_vel = velocity; - //vel = velocity; -// for (int i = 0; i < MVG_AVG; i++) { -// velocity = velocity + v_prev[i]; -// } -// velocity = velocity / (MVG_AVG + 1.0); - prev_pulses = curr_pulses; - motor_ctrl.setProcessValue(velocity); - d = motor_ctrl.compute(); - motor = 1.0 - d; - Thread::wait(interval*1000); -// pc.printf("Exposure: %d, Center: %d, Angle: %f\r\n", t_int, center, a); - } + control_interrupt.attach(control, interrupt_T); + + while (true); } \ No newline at end of file