Everything put together
Dependencies: FastAnalogIn MODSERIAL PID QEI RPCInterface Servo mbed-rtos mbed telemetry
Diff: main.cpp
- Revision:
- 5:7cba3ffc38bb
- Parent:
- 4:947c3634b649
- Child:
- 6:f7a2197dd8aa
diff -r 947c3634b649 -r 7cba3ffc38bb main.cpp --- a/main.cpp Fri Apr 01 00:34:18 2016 +0000 +++ b/main.cpp Fri Apr 01 01:26:59 2016 +0000 @@ -9,17 +9,19 @@ // ========= // 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 + +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); // ============= // Communication // ============= Serial pc(USBTX, USBRX); // USB connection -Serial bt(PTC4, PTC3); // BlueSMiRF connection -//int idx = 0; +//Serial bt(PTC4, PTC3); // BlueSMiRF connection char cmd; // Command char ch; char in[5]; @@ -93,87 +95,87 @@ // ================ // 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(); - } - } - } -} +//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() { @@ -189,6 +191,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); } @@ -287,13 +290,17 @@ motor_ctrl.setMode(1); // Initialize bluetooth - bt.baud(115200); +// bt.baud(115200); // Initialize communications thread - Thread communication_thread(communication); +// Thread communication_thread(communication); + + telemetry_obj.transmit_header(); + // Start motor controller while (true) { + telemetry_obj.do_io(); 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])); @@ -310,6 +317,6 @@ d = motor_ctrl.compute(); motor = 1.0 - d; wait(interval); - pc.printf("Exposure: %d, Center: %d, Angle: %f\r\n", t_int, center, a); +// pc.printf("Exposure: %d, Center: %d, Angle: %f\r\n", t_int, center, a); } } \ No newline at end of file