EE192 Team 4
One big fixed period control loop
Dependencies: FastAnalogIn MODSERIAL PID QEI RPCInterface Servo mbed-rtos mbed telemetry PinDetect
Fork of
Everything
by 
EE192 Team 4
Diff: main.cpp
- Revision:
- 6:f7a2197dd8aa
- Parent:
- 5:7cba3ffc38bb
- Child:
- 7:8229baaf1fbf
--- a/main.cpp Fri Apr 01 01:26:59 2016 +0000
+++ b/main.cpp Fri Apr 01 19:46:22 2016 +0000
@@ -9,19 +9,17 @@
// =========
// 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);
+//MODSERIAL telemetry_serial(PTC4, PTC3); // TX, RX
+//telemetry::MbedHal telemetry_hal(telemetry_serial); // Hardware Abstraction Layer
+//telemetry::Telemetry telemetry_obj(telemetry_hal); // Telemetry
// =============
// Communication
// =============
Serial pc(USBTX, USBRX); // USB connection
-//Serial bt(PTC4, PTC3); // BlueSMiRF connection
+Serial bt(PTC4, PTC3); // BlueSMiRF connection
+//int idx = 0;
char cmd; // Command
char ch;
char in[5];
@@ -95,87 +93,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() {
@@ -191,7 +189,6 @@
for (int i = 0; i < 128; i++) {
clk = 0;
img[i] = ao.read_u16();
- tele_linescan[i] = img[i];
clk = 1;
wait_us(1);
}
@@ -290,17 +287,13 @@
motor_ctrl.setMode(1);
// Initialize bluetooth
-// bt.baud(115200);
+ bt.baud(115200);
// Initialize communications thread
-// Thread communication_thread(communication);
-
- telemetry_obj.transmit_header();
-
+ Thread communication_thread(communication);
// 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]));
@@ -317,6 +310,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