EE192 Team 4
Fixed PWM
Dependencies: FastAnalogIn MODSERIAL PID QEI RPCInterface Servo mbed-rtos mbed telemetry
Fork of
Sequential_Timing
by 
EE192 Team 4
Revision 14:a0614f48e6ef, committed 2016-04-11
- Comitter:
- vsutardja
- Date:
- Mon Apr 11 23:56:23 2016 +0000
- Parent:
- 13:d6e167698517
- Child:
- 15:db95bb7c7f93
- Commit message:
- Race 1
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Fri Apr 08 23:43:12 2016 +0000
+++ b/main.cpp Mon Apr 11 23:56:23 2016 +0000
@@ -11,8 +11,8 @@
// =========
// Telemetry
// =========
-MODSERIAL telemetry_serial(PTC4, PTC3); // TX, RX
-//MODSERIAL telemetry_serial(USBTX, USBRX);
+//MODSERIAL telemetry_serial(PTC4, PTC3); // TX, RX
+MODSERIAL telemetry_serial(USBTX, USBRX);
telemetry::MbedHal telemetry_hal(telemetry_serial); // Hardware Abstraction Layer
telemetry::Telemetry telemetry_obj(telemetry_hal); // Telemetry
@@ -30,15 +30,16 @@
int t_steer = 0;
int t_vel = 0;
-float interrupt_T = 0.025;
+float interrupt_T = 0.015;
+bool ctrl_flag = false;
// =============
// Communication
// =============
char comm_cmd; // Command
-char comm_in[5]; // Input
-//Serial pc(USBTX, USBRX); // USB connection
-//Serial bt(PTC4, PTC3); // BlueSMiRF connection
+char comm_in[8]; // Input
+//Serial bt(USBTX, USBRX); // USB connection
+Serial bt(PTC4, PTC3); // BlueSMiRF connection
void communication(void const *args); // Communications
@@ -66,11 +67,12 @@
// ========
// Velocity
// ========
-float Kp = 6.0; // Proportional factor
+float Kp = 8.0; // Proportional factor
float Ki = 0; // Integral factor
float Kd = 0; // Derivative factor
float interval = 0.01; // Sampling interval
-float ref_v = 0.5; // Reference velocity
+float prev_vels[10];
+float ref_v = 0.8; // Reference velocity
PID motor_ctrl(Kp, Ki, Kd, interval); // Motor controller
// =====
@@ -115,197 +117,159 @@
e_stop = true;
motor = 1.0;
}
- if (serial->rxGetLastChar() == '+') {
- ref_v = ref_v + 0.2;
+ if (serial->rxGetLastChar() == '=') {
+ ref_v = ref_v + 0.05;
motor_ctrl.setSetPoint(ref_v);
}
if (serial->rxGetLastChar() == '-') {
- ref_v = ref_v - 0.2;
+ ref_v = ref_v - 0.05;
motor_ctrl.setSetPoint(ref_v);
}
}
// Communications
-void communication(void const *args) {
- telemetry_serial.baud(115200);
- telemetry_serial.attach(&killswitch, MODSERIAL::RxIrq);
- telemetry_obj.transmit_header();
- while (true) {
- tele_time_ms = t_tele.read_ms();
- telemetry_obj.do_io();
- }
-}
-
-//void kill(Arguments *input, Reply *output) {
-// output->putData("E_STOP\r\n");
-// motor = 1.0;
-// e_stop = true;
-//}
-
//void communication(void const *args) {
-// // Initialize bluetooth
-// bt.baud(115200);
-//
+// telemetry_serial.baud(115200);
+// telemetry_serial.attach(&killswitch, MODSERIAL::RxIrq);
+// 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] EMERGENCY STOP\r\n");
-// bt.printf(" [8] Timing\r\n");
-// comm_cmd = bt.getc();
-// while (comm_cmd != 'q') {
-// switch(atoi(&comm_cmd)){
-// case 0:
-// bt.printf("Duty cycle: %f, Pulse count: %d, Velocity: %f, Kp: %f, Ki: %f, Kd: %f\r\n", motor_duty, curr_pulses, velocity, Kp, Ki, Kd);
-// break;
-// case 1:
-// 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 (5 digits): ", Kp);
-// for (int i = 0; i < 5; i++) {
-// comm_in[i] = bt.getc();
-// bt.putc(comm_in[i]);
-// }
-// bt.printf("\r\n");
-// Kp = atof(comm_in);
-// motor_ctrl.setTunings(Kp, Ki, Kd);
-// comm_cmd = 'q';
-// break;
-// case 3:
-// bt.printf("Current: %f, New (5 digits): ", Ki);
-// for (int i = 0; i < 5; i++) {
-// comm_in[i] = bt.getc();
-// bt.putc(comm_in[i]);
-// }
-// bt.printf("\r\n");
-// Ki = atof(comm_in);
-// motor_ctrl.setTunings(Kp, Ki, Kd);
-// comm_cmd = 'q';
-// break;
-// case 4:
-// bt.printf("Current: %f, New (5 digits): ", Kd);
-// for (int i = 0; i < 5; i++) {
-// comm_in[i] = bt.getc();
-// bt.putc(comm_in[i]);
-// }
-// bt.printf("\r\n");
-// Kd = atof(comm_in);
-// motor_ctrl.setTunings(Kp, Ki, Kd);
-// comm_cmd = 'q';
-// break;
-// case 5:
-// bt.printf("Current: %f, New (5 digits): ", Ks);
-// for (int i = 0; i < 5; i++) {
-// comm_in[i] = bt.getc();
-// bt.putc(comm_in[i]);
-// }
-// bt.printf("\r\n");
-// Ks = atof(comm_in);
-// comm_cmd = 'q';
-// break;
-// case 6:
-// 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]);
-// }
-// bt.printf("\r\n");
-// ref_v = atof(comm_in);
-// motor_ctrl.setSetPoint(ref_v);
-// comm_cmd = 'q';
-// break;
-// case 7:
-// e_stop = true;
-// bt.printf("STOPPED\r\n");
-// 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;
-// }
-// if (bt.readable()) {
-// comm_cmd = bt.getc();
-// }
-// }
+// tele_time_ms = t_tele.read_ms();
+// telemetry_obj.do_io();
// }
//}
+void communication(void const *args) {
+ // Initialize bluetooth
+ bt.baud(115200);
+
+ 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] EMERGENCY STOP\r\n");
+ bt.printf(" [8] Timing\r\n");
+ comm_cmd = bt.getc();
+ while (comm_cmd != 'q') {
+ switch(atoi(&comm_cmd)){
+ case 0:
+ bt.printf("Duty cycle: %f, Pulse count: %d, Velocity: %f, Kp: %f, Ki: %f, Kd: %f\r\n", motor_duty, curr_pulses, velocity, Kp, Ki, Kd);
+ break;
+ case 1:
+ 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): ", Kp);
+ for (int i = 0; i < 8; i++) {
+ comm_in[i] = bt.getc();
+ bt.putc(comm_in[i]);
+ }
+ bt.printf("\r\n");
+ Kp = atof(comm_in);
+ motor_ctrl.setTunings(Kp, Ki, Kd);
+ comm_cmd = 'q';
+ break;
+ case 3:
+ bt.printf("Current: %f, New (8 digits): ", Ki);
+ for (int i = 0; i < 8; i++) {
+ comm_in[i] = bt.getc();
+ bt.putc(comm_in[i]);
+ }
+ bt.printf("\r\n");
+ Ki = atof(comm_in);
+ motor_ctrl.setTunings(Kp, Ki, Kd);
+ comm_cmd = 'q';
+ break;
+ case 4:
+ bt.printf("Current: %f, New (8 digits): ", Kd);
+ for (int i = 0; i < 8; i++) {
+ comm_in[i] = bt.getc();
+ bt.putc(comm_in[i]);
+ }
+ bt.printf("\r\n");
+ Kd = atof(comm_in);
+ motor_ctrl.setTunings(Kp, Ki, Kd);
+ comm_cmd = 'q';
+ break;
+ case 5:
+ bt.printf("Current: %f, New (8 digits): ", Ks);
+ for (int i = 0; i < 8; i++) {
+ comm_in[i] = bt.getc();
+ bt.putc(comm_in[i]);
+ }
+ bt.printf("\r\n");
+ Ks = atof(comm_in);
+ comm_cmd = 'q';
+ break;
+ case 6:
+ bt.printf("Current: %f, New (8 digits): ", ref_v);
+ for (int i = 0; i < 8; i++) {
+ comm_in[i] = bt.getc();
+ bt.putc(comm_in[i]);
+ }
+ bt.printf("\r\n");
+ ref_v = atof(comm_in);
+ motor_ctrl.setSetPoint(ref_v);
+ comm_cmd = 'q';
+ break;
+ case 7:
+ 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;
+ }
+ if (bt.readable()) {
+ comm_cmd = bt.getc();
+ }
+ }
+ }
+}
+
void control() {
// Image capture
- t.reset();
+ // t.reset();
// Dummy read
-// clk = 0;
PTD->PCOR = (0x01 << 5);
-// j = 0;
-// wait_us(1);
-// si = 1;
PTD->PSOR = (0x01);
-// j= 0;
-// wait_us(1);
-// clk = 1;
PTD->PSOR = (0x01 << 5);
-// j = 0;
-// wait_us(1);
-// si = 0;
PTD->PCOR = (0x01);
-// j = 0;
for (int i = 0; i < 128; i++) {
-// wait_us(1);
PTD->PCOR = (0x01 << 5);
-// j = 0;
-// wait_us(1);
PTD->PSOR = (0x01 << 5);
-// j = 0;
}
// Expose
wait_us(t_int);
// Read camera
-// clk = 0;
PTD->PCOR = (0x01 << 5);
-// j = 0;
-// wait_us(1);
-// si = 1;
PTD->PSOR = (0x01);
-// j = 0;
-// wait_us(1);
-// clk = 1;
PTD->PSOR = (0x01 << 5);
-// j = 0;
-// wait_us(1);
-// si = 0;
PTD->PCOR = (0x01);
-// j = 0;
-// wait_us(1);
for (int i = 0; i < 128; i++) {
-// clk = 0;
PTD->PCOR = (0x01 << 5);
-// j = 0;
if (i > 9 && i < 118) {
img[i-10] = ao.read_u16();
tele_linescan[i-10] = img[i-10];
}
- //} else {
-// wait_us(1);
-// }
PTD->PSOR = (0x01 << 5);
-// j = 0;
}
- t_cam = t.read_us();
+ // t_cam = t.read_us();
// Steering control
- t.reset();
+ // t.reset();
// Detect peak edges
j = 0;
@@ -372,15 +336,35 @@
}
tele_exposure = t_int;
- t_steer = t.read_us();
-
- // wait_us(8000 - t.read_us());
+ // t_steer = t.read_us();
// Velocity control
- t.reset();
+ // 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);
@@ -390,7 +374,12 @@
} else {
motor = 1.0;
}
- t_vel = t.read_us();
+ // t_vel = t.read_us();
+ ctrl_flag = false;
+}
+
+void set_control_flag() {
+ ctrl_flag = true;
}
// ====
@@ -402,13 +391,17 @@
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.01, 0.5);
+ motor_ctrl.setOutputLimits(0.0, 0.75);
motor_ctrl.setSetPoint(ref_v);
motor_ctrl.setBias(0.0);
motor_ctrl.setMode(1);
@@ -419,8 +412,13 @@
// Initialize communications thread
Thread communication_thread(communication);
-
- control_interrupt.attach(control, interrupt_T);
+
+// control_interrupt.attach(control, interrupt_T);
+ control_interrupt.attach(set_control_flag, interrupt_T);
- while (true);
+ while (true) {
+ if (ctrl_flag) {
+ control();
+ }
+ }
}
\ No newline at end of file