EE192 Team 4
Fixed PWM
Dependencies: FastAnalogIn MODSERIAL PID QEI RPCInterface Servo mbed-rtos mbed telemetry
Fork of
Sequential_Timing
by 
EE192 Team 4
Diff: main.cpp
- Revision:
- 16:3ab3c4670f4f
- Parent:
- 15:db95bb7c7f93
- Child:
- 17:bf6192a361ab
--- a/main.cpp Tue Apr 12 00:13:56 2016 +0000
+++ b/main.cpp Tue Apr 12 02:03:50 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
@@ -38,13 +38,13 @@
// =============
char comm_cmd; // Command
char comm_in[8]; // Input
-//Serial bt(USBTX, USBRX); // USB connection
-Serial bt(PTC4, PTC3); // BlueSMiRF connection
+Serial bt(USBTX, USBRX); // USB connection
+//Serial bt(PTC4, PTC3); // BlueSMiRF connection
void communication(void const *args); // Communications
-//void kill(Arguments *input, Reply *output);
-//RPCFunction rpc_kill(&kill, "kill");
+//void Kmill(Arguments *input, Reply *output);
+//RPCFunction rpc_Kmill(&Kmill, "Kmill");
// =====
// Motor
@@ -67,19 +67,22 @@
// ========
// Velocity
// ========
-float Kp = 8.0; // Proportional factor
-float Ki = 0; // Integral factor
-float Kd = 0; // Derivative factor
-float interval = 0.01; // Sampling interval
+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(Kp, Ki, Kd, interval); // Motor controller
+PID motor_ctrl(Kmp, Kmi, Kmd, interrupt_T); // Motor controller
// =====
// Servo
// =====
float angle = 88; // Angle
-float Ks = 0.9; // Steering proportion
+float Ksp = 1.0; // Steering proportion
+float Ksi = 0;
+float Ksd = 0;
+PID servo_ctrl(Ksp, Ksi, Ksd, interrupt_T);
Servo servo(PTA12); // Signal pin (PWM)
// ======
@@ -111,7 +114,7 @@
// ================
-void killswitch(MODSERIAL_IRQ_INFO *q) {
+void Kmillswitch(MODSERIAL_IRQ_INFO *q) {
MODSERIAL *serial = q->serial;
if (serial->rxGetLastChar() == 'k') {
e_stop = true;
@@ -130,7 +133,7 @@
// Communications
//void communication(void const *args) {
// telemetry_serial.baud(115200);
-// telemetry_serial.attach(&killswitch, MODSERIAL::RxIrq);
+// telemetry_serial.attach(&Kmillswitch, MODSERIAL::RxIrq);
// telemetry_obj.transmit_header();
// while (true) {
// tele_time_ms = t_tele.read_ms();
@@ -147,10 +150,10 @@
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(" [2] Change Kmp\r\n");
+ bt.printf(" [3] Change Kmi\r\n");
+ bt.printf(" [4] Change Kmd\r\n");
+ 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");
@@ -160,52 +163,52 @@
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);
+ bt.printf("Duty cycle: %f, Pulse count: %d, Velocity: %f, Kmp: %f, Kmi: %f, Kmd: %f\r\n", motor_duty, curr_pulses, velocity, Kmp, Kmi, Kmd);
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);
+ bt.printf("Current: %f, New (8 digits): ", Kmp);
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);
+ Kmp = atof(comm_in);
+ motor_ctrl.setTunings(Kmp, Kmi, Kmd);
comm_cmd = 'q';
break;
case 3:
- bt.printf("Current: %f, New (8 digits): ", Ki);
+ bt.printf("Current: %f, New (8 digits): ", Kmi);
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);
+ Kmi = atof(comm_in);
+ motor_ctrl.setTunings(Kmp, Kmi, Kmd);
comm_cmd = 'q';
break;
case 4:
- bt.printf("Current: %f, New (8 digits): ", Kd);
+ bt.printf("Current: %f, New (8 digits): ", Kmd);
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);
+ Kmd = atof(comm_in);
+ motor_ctrl.setTunings(Kmp, Kmi, Kmd);
comm_cmd = 'q';
break;
case 5:
- bt.printf("Current: %f, New (8 digits): ", Ks);
+ bt.printf("Current: %f, New (8 digits): ", Ksp);
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);
+ Ksp = atof(comm_in);
comm_cmd = 'q';
break;
case 6:
@@ -288,10 +291,10 @@
// Detect peak edges
j = 0;
- for (int i = 0; i < 107 && j < 5; i++) {
+ for (int i = 0; i < 108 && j < 5; i++) {
if (img[i] > 45000) {
left[j] = i;
- while (img[i] > 45000) {
+ while (img[i] > 45000 && i < 108) {
i = i + 1;
}
right[j] = i;
@@ -301,12 +304,12 @@
// Calculate peak centers
for (int i = 0; i < j; i++) {
- centers[i] = (left[i] + right[i] + 10) / 2;
+ centers[i] = (left[i] + right[i] + 20) / 2;
}
// Assign scores
for (int i = 0; i < j; i++) {
- scores[i] = 10 / (right[i] - left[i]) + img[centers[i]] / 65536 + 10 / abs(centers[i] - prev_center);
+ scores[i] = 8 / (right[i] - left[i]) + img[centers[i]] / 65536 + 5 / abs(centers[i] - prev_center);
}
// Choose most likely center
@@ -321,13 +324,17 @@
tele_center = center;
// Set servo angle
- angle = 88 + (55 - center) * Ks;
- if (angle > 113) {
- angle = 113;
- }
- if (angle < 63) {
- angle = 63;
- }
+ // 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
@@ -425,6 +432,13 @@
servo.calibrate(0.001, 45.0);
servo = angle / 180.0;
+ // Initialize servo controller
+ servo_ctrl.setInputLimits(10, 117);
+ servo_ctrl.setOutputLimits(-25, 25);
+ servo_ctrl.setSetPoint(63.5);
+ servo_ctrl.setBias(0.0);
+ servo_ctrl.setMode(1);
+
// Initialize communications thread
Thread communication_thread(communication);