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 4:947c3634b649, committed 2016-04-01
- Comitter:
- vsutardja
- Date:
- Fri Apr 01 00:34:18 2016 +0000
- Parent:
- 3:c57674c348bd
- Child:
- 5:7cba3ffc38bb
- Commit message:
- Faster Figure 8
Changed in this revision
| RPCInterface.lib | Show annotated file Show diff for this revision Revisions of this file |
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/RPCInterface.lib Fri Apr 01 00:34:18 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/MichaelW/code/RPCInterface/#9d82e28ffaea
--- a/main.cpp Wed Mar 30 01:21:39 2016 +0000
+++ b/main.cpp Fri Apr 01 00:34:18 2016 +0000
@@ -22,7 +22,7 @@
//int idx = 0;
char cmd; // Command
char ch;
-char in[2];
+char in[5];
void communication(void const *args); // Communications
@@ -43,15 +43,17 @@
int prev_pulses = 0; // Previous pulse count
int curr_pulses = 0; // Current pulse count
float velocity = 0; // Velocity
+float vel = 0;
float v_prev[MVG_AVG] = {0};
// ========
// Velocity
// ========
-float Kp = 13.0; // Proportional factor
+float Kp = 6.0; // Proportional factor
float Ki = 0; // Integral factor
float Kd = 0; // Derivative factor
float interval = 0.01; // Sampling interval
+float ref_v = 1.0;
PID motor_ctrl(Kp, Ki, Kd, interval); // Motor controller
// =====
@@ -59,6 +61,7 @@
// =====
Servo servo(PTA12); // Enable pin (PWM)
float a = 88; // Angle
+float Ks = 0.7;
// ======
// Camera
@@ -81,7 +84,7 @@
int argmax = 0;
int argmin = 0;
int temp[128];
-int center;
+int center = 64;
void track(); // Line-tracking steering
@@ -97,39 +100,72 @@
bt.printf(" [0] Velocity\r\n");
bt.printf(" [1] Steering\r\n");
bt.printf(" [2] Change Kp\r\n");
- bt.printf(" [3] Change exposure time\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\r\n", d, curr_pulses, velocity, Kp);
+ 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:
-// idx = 0;
- bt.printf("Current Kp = %f. Enter new Kp: ", Kp);
-// while ((cmd = bt.getc()) != '\n') {
- for (int idx = 0; idx < 2; idx++) {
- ch = bt.getc();
- in[idx] = ch;
-// idx = idx + 1;
+ 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:
-// idx = 0;
- bt.printf("Current t_int = %dms. Enter new t_int (ms): ", t_int / 1000);
-// while ((cmd = bt.getc()) != '\n') {
- for (int idx = 0; idx < 2; idx++) {
- ch = bt.getc();
- in[idx] = ch;
-// idx = idx + 1;
+ 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");
- t_int = atoi(in);
+ 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()) {
@@ -200,26 +236,26 @@
}
}
- for (int i = 0; i < 30; i++) {
- lum_bg = lum_bg + img[64 - 10 - i] / 60.0 + img[64 + 10 + i] / 60.0;
+ for (int i = 0; i < 10; i++) {
+ lum_bg = lum_bg + img[64 - 4 - i] / 20.0 + img[64 + 4 + i] / 20.0;
}
contrast = (max - lum_bg) / lum_bg;
- if (contrast < 1.5) {
+// if (contrast < 1.5) {
// Underexposed
- if (max < 55000) {
- t_int = t_int + (55000 - max);
+ if (max < 60000) {
+ t_int = t_int + 0.15 * (60000 - max);
}
// Overexposed
if (lum_bg > 25000) {
- t_int = t_int - (lum_bg - 25000);
+ t_int = t_int - 0.15 * (lum_bg - 25000);
}
- }
+// }
if (max > 43253) {
center = (argmax + argmin + 2 + 11) / 2;
- a = 88 + (64 - center) * 0.625;
+ a = 88 + (64 - center) * Ks;
servo = a / 180;
}
@@ -245,8 +281,8 @@
// Initialize motor controller
motor_ctrl.setInputLimits(0.0, 10.0);
- motor_ctrl.setOutputLimits(0.1, 0.5);
- motor_ctrl.setSetPoint(1.5);
+ motor_ctrl.setOutputLimits(0.01, 0.5);
+ motor_ctrl.setSetPoint(ref_v);
motor_ctrl.setBias(0.0);
motor_ctrl.setMode(1);
@@ -259,20 +295,21 @@
// Start motor controller
while (true) {
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;
- for (int i = 0; i < MVG_AVG; i++) {
- velocity = velocity + v_prev[i];
- }
- velocity = velocity / (MVG_AVG + 1.0);
+ //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;
+ //vel = velocity;
+// for (int i = 0; i < MVG_AVG; i++) {
+// velocity = velocity + v_prev[i];
+// }
+// velocity = velocity / (MVG_AVG + 1.0);
prev_pulses = curr_pulses;
motor_ctrl.setProcessValue(velocity);
d = motor_ctrl.compute();
motor = 1.0 - d;
wait(interval);
- pc.printf("BG Luminosity: %f, Feature Luminosity: %d, Contrast: %f, Exposure: %d\r\n", lum_bg, max, contrast, t_int);
+ pc.printf("Exposure: %d, Center: %d, Angle: %f\r\n", t_int, center, a);
}
}
\ No newline at end of file