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:
- 17:bf6192a361ab
- Parent:
- 16:3ab3c4670f4f
--- a/main.cpp Tue Apr 12 02:03:50 2016 +0000
+++ b/main.cpp Thu Apr 14 00:45:14 2016 +0000
@@ -11,6 +11,8 @@
// =========
// Telemetry
// =========
+//DigitalOut test(PTD1);
+
MODSERIAL telemetry_serial(PTC4, PTC3); // TX, RX
//MODSERIAL telemetry_serial(USBTX, USBRX);
telemetry::MbedHal telemetry_hal(telemetry_serial); // Hardware Abstraction Layer
@@ -26,9 +28,11 @@
Timer t;
Timer t_tele;
Ticker control_interrupt;
-int t_cam = 0;
+int t_cam1 = 0;
+int t_cam2 = 0;
int t_steer = 0;
int t_vel = 0;
+int t_down = 0;
float interrupt_T = 0.015;
bool ctrl_flag = false;
@@ -37,15 +41,12 @@
// Communication
// =============
char comm_cmd; // Command
-char comm_in[8]; // Input
+char comm_in[5]; // Input
Serial bt(USBTX, USBRX); // USB connection
//Serial bt(PTC4, PTC3); // BlueSMiRF connection
void communication(void const *args); // Communications
-//void Kmill(Arguments *input, Reply *output);
-//RPCFunction rpc_Kmill(&Kmill, "Kmill");
-
// =====
// Motor
// =====
@@ -60,6 +61,7 @@
const int ppr = 389; // Pulses per revolution
const float c = 0.20106; // Wheel circumference
int prev_pulses = 0; // Previous pulse count
+int prev_pulse_counts[5] = {0};
int curr_pulses = 0; // Current pulse count
float velocity = 0; // Velocity
QEI qei(PTD3, PTD2, NC, ppr, QEI::X4_ENCODING); // Quadrature encoder
@@ -67,19 +69,19 @@
// ========
// Velocity
// ========
-float Kmp = 8.0; // Proportional factor
-float Kmi = 0; // Integral factor
-float Kmd = 0; // Derivative factor
-float motor_interval = 0.01; // Sampling interval
-float prev_vels[10];
-float ref_v = 0.8; // Reference velocity
-PID motor_ctrl(Kmp, Kmi, Kmd, interrupt_T); // Motor controller
+float Kmp = 36.0; // Proportional factor
+float Kmi = 1; // Integral factor
+float Kmd = 0; // Derivative factor
+float interval = 0.01; // Sampling interval
+float ref_v = 0.5; // Reference velocity
+int vel_count = 1;
+PID motor_ctrl(Kmp, Kmi, Kmd, interrupt_T); // Motor controller
// =====
// Servo
// =====
float angle = 88; // Angle
-float Ksp = 1.0; // Steering proportion
+float Ksp = 1; // Steering proportion
float Ksi = 0;
float Ksd = 0;
PID servo_ctrl(Ksp, Ksi, Ksd, interrupt_T);
@@ -89,7 +91,7 @@
// Camera
// ======
int t_int = 10000; // Exposure time
-const int T_INT_MAX = interrupt_T * 1000000 - 1000; // Maximum exposure time
+const int T_INT_MAX = interrupt_T * 1000000 - 2000; // Maximum exposure time
const int T_INT_MIN = 35; // Minimum exposure time
int img[108] = {0}; // Image data
DigitalOut clk(PTD5); // Clock pin
@@ -113,19 +115,18 @@
// Functions
// ================
-
-void Kmillswitch(MODSERIAL_IRQ_INFO *q) {
+void killswitch(MODSERIAL_IRQ_INFO *q) {
MODSERIAL *serial = q->serial;
if (serial->rxGetLastChar() == 'k') {
e_stop = true;
motor = 1.0;
}
if (serial->rxGetLastChar() == '+') {
- ref_v = ref_v + 0.05;
+ ref_v = ref_v + 0.2;
motor_ctrl.setSetPoint(ref_v);
}
if (serial->rxGetLastChar() == '-') {
- ref_v = ref_v - 0.05;
+ ref_v = ref_v - 0.2;
motor_ctrl.setSetPoint(ref_v);
}
}
@@ -133,7 +134,7 @@
// Communications
//void communication(void const *args) {
// telemetry_serial.baud(115200);
-// telemetry_serial.attach(&Kmillswitch, MODSERIAL::RxIrq);
+// telemetry_serial.attach(&killswitch, MODSERIAL::RxIrq);
// telemetry_obj.transmit_header();
// while (true) {
// tele_time_ms = t_tele.read_ms();
@@ -156,9 +157,7 @@
bt.printf(" [5] Change Ksp\r\n");
bt.printf(" [6] Change reference velocity\r\n");
bt.printf(" [7] EMERGENCY STOP\r\n");
-// bt.printf(" [8] Timing\r\n");
- bt.printf(" [8] duty += 0.05\r\n");
- bt.printf(" [9] duty -= 0.05\r\n");
+ bt.printf(" [8] Timing\r\n");
comm_cmd = bt.getc();
while (comm_cmd != 'q') {
switch(atoi(&comm_cmd)){
@@ -169,8 +168,8 @@
bt.printf("Servo angle: %f, Track center: %d, t_int: %d\r\n", angle, center, t_int);
break;
case 2:
- bt.printf("Current: %f, New (8 digits): ", Kmp);
- for (int i = 0; i < 8; i++) {
+ bt.printf("Current: %f, New (5 digits): ", Kmp);
+ for (int i = 0; i < 5; i++) {
comm_in[i] = bt.getc();
bt.putc(comm_in[i]);
}
@@ -180,8 +179,8 @@
comm_cmd = 'q';
break;
case 3:
- bt.printf("Current: %f, New (8 digits): ", Kmi);
- for (int i = 0; i < 8; i++) {
+ bt.printf("Current: %f, New (5 digits): ", Kmi);
+ for (int i = 0; i < 5; i++) {
comm_in[i] = bt.getc();
bt.putc(comm_in[i]);
}
@@ -191,8 +190,8 @@
comm_cmd = 'q';
break;
case 4:
- bt.printf("Current: %f, New (8 digits): ", Kmd);
- for (int i = 0; i < 8; i++) {
+ bt.printf("Current: %f, New (5 digits): ", Kmd);
+ for (int i = 0; i < 5; i++) {
comm_in[i] = bt.getc();
bt.putc(comm_in[i]);
}
@@ -202,8 +201,8 @@
comm_cmd = 'q';
break;
case 5:
- bt.printf("Current: %f, New (8 digits): ", Ksp);
- for (int i = 0; i < 8; i++) {
+ bt.printf("Current: %f, New (5 digits): ", Ksp);
+ for (int i = 0; i < 5; i++) {
comm_in[i] = bt.getc();
bt.putc(comm_in[i]);
}
@@ -212,8 +211,8 @@
comm_cmd = 'q';
break;
case 6:
- bt.printf("Current: %f, New (8 digits): ", ref_v);
- for (int i = 0; i < 8; i++) {
+ bt.printf("Current: %f, New (5 digits): ", ref_v);
+ for (int i = 0; i < 5; i++) {
comm_in[i] = bt.getc();
bt.putc(comm_in[i]);
}
@@ -223,25 +222,12 @@
comm_cmd = 'q';
break;
case 7:
-// e_stop = true;
- motor = 1.0;
+ e_stop = true;
bt.printf("STOPPED\r\n");
comm_cmd = 'q';
break;
-// case 8:
-// bt.printf("Exposure: %dus, Camera Read: %dus, Steering: %dus, Velocity: %dus, Total: %dus\r\n", t_int, t_cam-t_int, t_steer, t_vel, t_cam+t_steer+t_vel);
-// break;
case 8:
- motor_duty = motor_duty + 0.05;
- motor = 1.0 - motor_duty;
- bt.printf("%f\r\n", motor_duty);
- comm_cmd = 'q';
- break;
- case 9:
- motor_duty = motor_duty - 0.05;
- motor = 1.0 - motor_duty;
- bt.printf("%f\r\n", motor_duty);
- comm_cmd = 'q';
+ bt.printf("Read 1: %dus, Exposure: %dus, Read 2: %dus, Steering: %dus, Velocity: %dus, Down: %dus, Total: %dus\r\n", t_cam1, t_int, t_cam2, t_steer, t_vel, t_down, t_cam1+t_int+t_cam2+t_steer+t_vel+t_down);
break;
}
if (bt.readable()) {
@@ -253,7 +239,8 @@
void control() {
// Image capture
- // t.reset();
+ t_down = t.read_us();
+ t.reset();
// Dummy read
PTD->PCOR = (0x01 << 5);
@@ -265,6 +252,7 @@
PTD->PCOR = (0x01 << 5);
PTD->PSOR = (0x01 << 5);
}
+ t_cam1 = t.read_us();
// Expose
wait_us(t_int);
@@ -275,6 +263,7 @@
PTD->PSOR = (0x01 << 5);
PTD->PCOR = (0x01);
+ t.reset();
for (int i = 0; i < 128; i++) {
PTD->PCOR = (0x01 << 5);
if (i > 9 && i < 118) {
@@ -284,58 +273,66 @@
PTD->PSOR = (0x01 << 5);
}
- // t_cam = t.read_us();
+ t_cam2 = t.read_us();
// Steering control
- // t.reset();
+ t.reset();
// Detect peak edges
j = 0;
for (int i = 0; i < 108 && j < 5; i++) {
- if (img[i] > 45000) {
+ if (img[i] > max * 0.65) {
left[j] = i;
- while (img[i] > 45000 && i < 108) {
- i = i + 1;
+ while (img[i] > max * 0.65) {
+ if (i < 107) {
+ i = i + 1;
+ } else {
+ j = j - 1;
+ break;
+ }
}
- right[j] = i;
+ if (i < 107) {
+ right[j] = i;
+ }
j = j + 1;
}
}
- // Calculate peak centers
- for (int i = 0; i < j; i++) {
- centers[i] = (left[i] + right[i] + 20) / 2;
- }
- // Assign scores
- for (int i = 0; i < j; i++) {
- scores[i] = 8 / (right[i] - left[i]) + img[centers[i]] / 65536 + 5 / abs(centers[i] - prev_center);
- }
-
- // Choose most likely center
- best_score_idx = 0;
- for (int i = 0; i < j; i++) {
- if (scores[i] > scores[best_score_idx]) {
- best_score_idx = i;
+ if (j > 0) {
+ // Calculate peak centers
+ for (int i = 0; i < j; i++) {
+ centers[i] = (left[i] + right[i] + 20) / 2;
+ }
+
+ // Assign scores
+ for (int i = 0; i < j; i++) {
+ scores[i] = 4 / (right[i] - left[i]) + img[centers[i]] / 65536 + 16 / abs(centers[i] - prev_center);
}
+
+ // Choose most likely center
+ best_score_idx = 0;
+ for (int i = 0; i < j; i++) {
+ if (scores[i] > scores[best_score_idx]) {
+ best_score_idx = i;
+ }
+ }
+ prev_center = center;
+ center = centers[best_score_idx];
+ tele_center = center;
+
+ angle = 88 + (64 - center) * 0.9;
+ if (angle > 113) {
+ angle = 113;
+ }
+ if (angle < 63) {
+ angle = 63;
+ }
+ // servo_ctrl.setProcessValue(center);
+ // angle = 88 + servo_ctrl.compute();
+ // servo = floor(angle / 180 * 100 + 0.5) / 100;
+ servo = angle / 180;
}
- prev_center = center;
- center = centers[best_score_idx];
- tele_center = center;
-
- // Set servo angle
- // angle = 88 + (55 - center) * Ksp;
- // if (angle > 113) {
- // angle = 113;
- // }
- // if (angle < 63) {
- // angle = 63;
- // }
- // servo = angle / 180;
-
- servo_ctrl.setProcessValue(center);
- angle = 88 + servo_ctrl.compute();
- servo = angle / 180;
// AGC
max = -1;
@@ -358,49 +355,43 @@
}
tele_exposure = t_int;
- // t_steer = t.read_us();
+ t_steer = t.read_us();
+
+ // wait_us(8000 - t.read_us());
+
+
-// // Velocity control
-// // t.reset();
-// if (!e_stop) {
-// curr_pulses = qei.getPulses();
-// //for (int i = 0; i < 9; i++) {
-//// prev_vels[i] = prev_vels[i+1];
-//// }
-//// prev_vels[9] = (curr_pulses - prev_pulses) / t.read() / ppr * c;
-//// t.reset();
-//// if (prev_vels[9] < 0) {
-//// prev_vels[9] = 0;
-//// }
-//// if (prev_vels[0] < 0) {
-//// velocity = prev_vels[9];
-//// } else {
-//// velocity = 0;
-//// for (int i = 0; i < 10; i++) {
-//// velocity = velocity + prev_vels[i];
-//// velocity = velocity / 10;
-//// }
-//// }
-// velocity = (curr_pulses - prev_pulses) / interrupt_T / ppr * c;
-// if (velocity < 0) {
-// velocity = 0;
-// }
-//// velocity = (curr_pulses - prev_pulses) / t.read() / ppr * c;
-// t.reset();
-// tele_velocity = velocity;
-// prev_pulses = curr_pulses;
-// motor_ctrl.setProcessValue(velocity);
-// motor_duty = motor_ctrl.compute();
-// motor = 1.0 - motor_duty;
-// tele_pwm = motor_duty;
-// } else {
-// motor = 1.0;
-// }
-// // t_vel = t.read_us();
-// ctrl_flag = false;
+ // Velocity control
+ t.reset();
+ if (!e_stop) {
+ curr_pulses = qei.getPulses();
+ if (vel_count < 6) {
+ velocity = curr_pulses / interrupt_T / vel_count / ppr * c;
+ prev_pulse_counts[vel_count - 1] = curr_pulses;
+ vel_count = vel_count + 1;
+ } else {
+ velocity = (curr_pulses - prev_pulse_counts[0]) / interrupt_T / 5 / ppr * c;
+ for (int i = 0; i < 4; i++) {
+ prev_pulse_counts[i] = prev_pulse_counts[i+1];
+ }
+ prev_pulse_counts[4] = curr_pulses;
+ }
+ tele_velocity = velocity;
+ motor_ctrl.setProcessValue(velocity);
+ motor_duty = motor_ctrl.compute();
+ motor = 1.0 - motor_duty;
+ tele_pwm = motor_duty;
+ } else {
+ motor = 1.0;
+ }
+ t_vel = t.read_us();
+ t.reset();
+ ctrl_flag = false;
+// test = 0;
}
-void set_control_flag() {
+void set_ctrl_flag() {
+// test = 1;
ctrl_flag = true;
}
@@ -413,17 +404,13 @@
tele_center.set_limits(0, 128);
tele_pwm.set_limits(0.0, 1.0);
- for (int i = 0; i < 10; i++) {
- prev_vels[i] = -1;
- }
-
// Initialize motor
motor.period_us(motor_T);
motor = 1.0 - motor_duty;
// Initialize motor controller
motor_ctrl.setInputLimits(0.0, 10.0);
- motor_ctrl.setOutputLimits(0.0, 0.75);
+ motor_ctrl.setOutputLimits(0.0, 0.5);
motor_ctrl.setSetPoint(ref_v);
motor_ctrl.setBias(0.0);
motor_ctrl.setMode(1);
@@ -436,18 +423,19 @@
servo_ctrl.setInputLimits(10, 117);
servo_ctrl.setOutputLimits(-25, 25);
servo_ctrl.setSetPoint(63.5);
- servo_ctrl.setBias(0.0);
+ servo_ctrl.setBias(0);
servo_ctrl.setMode(1);
// Initialize communications thread
- Thread communication_thread(communication);
-
- control_interrupt.attach(control, interrupt_T);
-// control_interrupt.attach(set_control_flag, interrupt_T);
+// Thread communication_thread(communication);
+
+ // control_interrupt.attach(control, interrupt_T);
+ // Thread::wait(osWaitForever);
+ control_interrupt.attach(set_ctrl_flag, interrupt_T);
while (true) {
-// if (ctrl_flag) {
-// control();
-// }
+ if (ctrl_flag) {
+ control();
+ }
}
}
\ No newline at end of file