Fixed PWM
Dependencies: FastAnalogIn MODSERIAL PID QEI RPCInterface Servo mbed-rtos mbed telemetry
Fork of Sequential_Timing by
Diff: main.cpp
- Revision:
- 11:4348bba086a4
- Parent:
- 10:716484b1ddb5
- Child:
- 12:54e7d8ff3a74
--- a/main.cpp Tue Apr 05 18:30:41 2016 +0000 +++ b/main.cpp Tue Apr 05 21:52:32 2016 +0000 @@ -6,29 +6,33 @@ #include "QEI.h" #include "telemetry.h" -//extern "C" { extern int printfNB(const char *format, ...); } -//extern "C" { extern int putcharNB(int);} - // ========= // 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 +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 -MODSERIAL bt(PTC4, PTC3); // BlueSMiRF connection -//int idx = 0; +//Serial bt(PTC4, PTC3); // BlueSMiRF connection char cmd; // Command char ch; char in[5]; void communication(void const *args); // Communications -int lock = 0; // ===== // Motor @@ -56,7 +60,7 @@ float Kp = 6.0; // Proportional factor float Ki = 0; // Integral factor float Kd = 0; // Derivative factor -float interval = 0.01; // Sampling interval +float interval = 0.1; // Sampling interval float ref_v = 1.0; PID motor_ctrl(Kp, Ki, Kd, interval); // Motor controller @@ -65,7 +69,7 @@ // ===== Servo servo(PTA12); // Enable pin (PWM) float a = 88; // Angle -float Ks = 0.7; +float Ks = 0.9; // ====== // Camera @@ -74,9 +78,11 @@ 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 -uint8_t img_out[128]; void readCamera(); // Read data from camera @@ -97,105 +103,122 @@ // Functions // ================ -// Communications -void communication(void const *args) { +void tele_comm(void const *args) { + telemetry_serial.baud(115200); + telemetry_obj.transmit_header(); 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"); - bt.printf(" [7] Checkpoint telemetry\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; -// case 7: -// while (lock == 0); -// for (int i = 0; i < 128; i++) { -// bt.printf("%d, ", img[i]); -// } -// bt.printf("%d\r\n", center); -// lock = 0; -// break; - } - if (bt.readable()) { - cmd = bt.getc(); - } - } + 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() { - // Start data transfer - //if (lock == 1) { + //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; @@ -207,6 +230,7 @@ for (int i = 0; i < 128; i++) { clk = 0; img[i] = ao.read_u16(); + tele_linescan[i] = img[i]; clk = 1; wait_us(1); } @@ -255,36 +279,64 @@ } for (int i = 0; i < 10; i++) { - lum_bg = lum_bg + img[64 - 4 - i] / 20.0 + img[64 + 4 + i] / 20.0; + 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); - } + // 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; // } - if (max > 43253) { + tele_cam_dec = cam_dec; + + if (max > 43253 && argmax < argmin) { center = (argmax + argmin + 2 + 11) / 2; - a = 88 + (64 - center) * Ks; + 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); -// lock = 1; } // ==== // Main // ==== int main() { +// osThreadSetPriority(osThreadGetId(), osPriorityRealTime); + t.start(); // Initialize motor motor.period_us(T); @@ -306,19 +358,31 @@ motor_ctrl.setMode(1); // Initialize bluetooth - bt.baud(115200); +// bt.baud(115200); + +// osThreadSetPriority(osThreadGetId(), osPriorityAboveNormal); // Initialize communications thread - Thread communication_thread(communication); +// 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]; @@ -328,7 +392,7 @@ motor_ctrl.setProcessValue(velocity); d = motor_ctrl.compute(); motor = 1.0 - d; - wait(interval); - pc.printf("Exposure: %d, Center: %d, Angle: %f\r\n", t_int, center, a); + Thread::wait(interval*1000); +// pc.printf("Exposure: %d, Center: %d, Angle: %f\r\n", t_int, center, a); } } \ No newline at end of file