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:
- 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