![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
One big fixed period control loop
Dependencies: FastAnalogIn MODSERIAL PID QEI RPCInterface Servo mbed-rtos mbed telemetry PinDetect
Fork of Everything by
main.cpp
- Committer:
- vsutardja
- Date:
- 2016-04-05
- Revision:
- 11:4348bba086a4
- Parent:
- 10:716484b1ddb5
- Child:
- 12:54e7d8ff3a74
File content as of revision 11:4348bba086a4:
#include "mbed.h" #include "rtos.h" #include "Servo.h" #include "FastAnalogIn.h" #include "PID.h" #include "QEI.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 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); // ============= // Communication // ============= Serial pc(USBTX, USBRX); // USB connection //Serial bt(PTC4, PTC3); // BlueSMiRF connection char cmd; // Command char ch; char in[5]; void communication(void const *args); // Communications // ===== // Motor // ===== PwmOut motor(PTA4); // Enable pin (PWM) int T = 25000; // Frequency float d = 0.0; // Duty cycle // ======= // 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}; // ======== // 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 // ===== // Servo // ===== Servo servo(PTA12); // Enable pin (PWM) float a = 88; // Angle float Ks = 0.9; // ====== // 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 // ================ // 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 // ================ // Functions // ================ void tele_comm(void const *args) { telemetry_serial.baud(115200); telemetry_obj.transmit_header(); while (true) { tele_time_ms = t.read_ms(); telemetry_obj.do_io(); Thread::wait(100); } } // Communications //void communication(void const *args) { // while (true) { // bt.printf("\r\nPress q to return to this prompt.\r\n"); // bt.printf("Available diagnostics:\r\n"); // bt.printf(" [0] Velocity\r\n"); // bt.printf(" [1] Steering\r\n"); // bt.printf(" [2] Change Kp\r\n"); // bt.printf(" [3] Change Ki\r\n"); // 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)){ // 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); // break; // case 1: // bt.printf("Servo angle: %f, Track center: %d, t_int: %d\r\n", a, 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]); // } // bt.printf("\r\n"); // Kp = atof(in); // motor_ctrl.setTunings(Kp, Ki, Kd); // 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]); // } // bt.printf("\r\n"); // Ki = atof(in); // motor_ctrl.setTunings(Kp, Ki, Kd); // 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]); // } // bt.printf("\r\n"); // Kd = atof(in); // motor_ctrl.setTunings(Kp, Ki, Kd); // 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]); // } // bt.printf("\r\n"); // Ks = atof(in); // 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]); // } // bt.printf("\r\n"); // ref_v = atof(in); // motor_ctrl.setSetPoint(ref_v); // cmd = 'q'; // break; // } // if (bt.readable()) { // 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; // Start data transfer si = 1; wait_us(1); clk = 1; wait_us(1); 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]; clk = 1; wait_us(1); } clk = 0; // Update servo angle track(); // 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; } 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; } } } } for (int i = 0; i < 10; i++) { lum_bg = lum_bg + img[55 - 4 - i] / 20.0 + img[55 + 4 + i] / 20.0; } contrast = (max - lum_bg) / lum_bg; // 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); // } // } 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; } tele_t_int = t_int; //if (t_int < T_INT_MIN) { // cam_dec = T_INT_MIN / t_int; // } tele_cam_dec = cam_dec; 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; } // camera_read.attach_us(&read_camera, 1000); camera_read.attach_us(&read_camera, t_int); } // ==== // Main // ==== int main() { // osThreadSetPriority(osThreadGetId(), osPriorityRealTime); t.start(); // 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(); // Initialize motor controller motor_ctrl.setInputLimits(0.0, 10.0); motor_ctrl.setOutputLimits(0.01, 0.5); motor_ctrl.setSetPoint(ref_v); motor_ctrl.setBias(0.0); motor_ctrl.setMode(1); // Initialize bluetooth // bt.baud(115200); // osThreadSetPriority(osThreadGetId(), osPriorityAboveNormal); // Initialize communications thread // Thread communication_thread(communication); Thread tele_comm_thread(tele_comm); // tele_comm_thread.set_priority(osPriorityBelowNormal); // 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); } }