EE192 Team 4
Possibly faster steering response.
Dependencies: FastAnalogIn MODSERIAL PID QEI RPCInterface Servo mbed-rtos mbed telemetry
Fork of
FixedPWMWill
by 
Will Porter
Revision 7:8229baaf1fbf, committed 2016-04-01
- Comitter:
- vsutardja
- Date:
- Fri Apr 01 20:41:10 2016 +0000
- Parent:
- 6:f7a2197dd8aa
- Child:
- 8:71c39d2f80e2
- Commit message:
- Telemetry fixed
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Fri Apr 01 19:46:22 2016 +0000
+++ b/main.cpp Fri Apr 01 20:41:10 2016 +0000
@@ -9,17 +9,22 @@
// =========
// 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::
// =============
// 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 +98,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 +194,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);
}
@@ -266,6 +272,7 @@
// Main
// ====
int main() {
+ t.start();
// Initialize motor
motor.period_us(T);
@@ -275,6 +282,11 @@
servo.calibrate(0.001, 45.0);
servo = a / 180.0;
+ wait(5);
+ telemetry_serial.baud(115200);
+ telemetry_serial.printf("t_h\r\n");
+ telemetry_obj.transmit_header();
+
// Initialize & start camera
clk = 0;
read_camera();
@@ -287,13 +299,21 @@
motor_ctrl.setMode(1);
// Initialize bluetooth
- bt.baud(115200);
+// bt.baud(115200);
// Initialize communications thread
- Thread communication_thread(communication);
+// Thread communication_thread(communication);
+
// Start motor controller
while (true) {
+// telemetry_serial.printf("%d\r\n", t.read_ms());
+ if (dec == 10) {
+ 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]));
@@ -310,6 +330,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