EE192 Team 4
Possibly faster steering response.
Dependencies: FastAnalogIn MODSERIAL PID QEI RPCInterface Servo mbed-rtos mbed telemetry
Fork of
FixedPWMWill
by 
Will Porter
Diff: main.cpp
- Revision:
- 22:b9969eaf0e80
- Parent:
- 20:8f4838b0d94d
- Child:
- 24:d206fa06c610
--- a/main.cpp Wed Apr 20 21:30:19 2016 +0000
+++ b/main.cpp Fri Apr 22 19:36:38 2016 +0000
@@ -52,10 +52,15 @@
// =====
// Motor
// =====
-const int motor_T = 25000; // Frequency
+const float motor_T = 1.0 / 100; // Frequency
float motor_duty = 0.0; // Duty cycle
+float ref_pwm = 0.0;
bool e_stop = false; // Emergency stop
+InterruptIn bemf_int(PTA4);
PwmOut motor(PTA4); // Enable pin (PWM)
+Timeout bemf_timeout;
+int bemf_vel = 0;
+int motor_ctrl_count = 0;
// =======
// Encoder
@@ -70,14 +75,20 @@
// ========
// Velocity
// ========
-float Kmp = 8.0; // Proportional factor
+float Kmp = 12.0; // Proportional factor
float Kmi = 0; // Integral factor
float Kmd = 0; // Derivative factor
float interval = 0.01; // Sampling interval
float prev_vels[10];
-float ref_v = 0.8; // Reference velocity
+float ref_v = 1.5; // Reference velocity
PID motor_ctrl(Kmp, Kmi, Kmd, interrupt_T); // Motor controller
+int turn_thresh = 15;
+float turn_gain = 0.015;
+float turn_minpwm = 0.1;
+
+FastAnalogIn bemf(PTB3, 0);
+
// =====
// Servo
// =====
@@ -171,20 +182,22 @@
bt.printf(" [2] Change Ksp\r\n");
bt.printf(" [3] Change Ksi\r\n");
bt.printf(" [4] Change Ksd\r\n");
- bt.printf(" [5] Change Ksp1\r\n");
+// bt.printf(" [5] Change Ksp1\r\n");
+ bt.printf(" [5] Change turn slowing minimum pwm\r\n");
// bt.printf(" [6] Change reference velocity\r\n");
- bt.printf(" [6] Change switching pixel threshold\r\n");
- bt.printf(" [7] EMERGENCY STOP\r\n");
+ bt.printf(" [6] Change turn slowing gain\r\n");
+// bt.printf(" [7] EMERGENCY STOP\r\n");
+ bt.printf(" [7] Change turn slowing dead zone\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] ref_pwm += 0.05\r\n");
+ bt.printf(" [9] ref_pwm -= 0.05\r\n");
comm_cmd = bt.getc();
while (comm_cmd != 'q') {
t.reset();
t.start();
switch(atoi(&comm_cmd)){
case 0:
- 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);
+ bt.printf("bemf: %d, Duty cycle: %f, Pulse count: %d, Velocity: %f, Kmp: %f, Kmi: %f, Kmd: %f\r\n", bemf_vel, motor_duty, curr_pulses, velocity, Kmp, Kmi, Kmd);
break;
case 1:
bt.printf("Servo angle: %f, Track center: %d, t_int: %d,rdyFlag: %d, dataFlag: %d, fired: %d, timer: %d\r\n", angle, center, t_int,rdyFlag,dataFlag,fired,t_coms);
@@ -258,25 +271,35 @@
// motor_ctrl.setTunings(Kmp, Kmi, Kmd);
// comm_cmd = 'q';
// break;
+// case 5:
+// bt.printf("Current: %f, New (8 digits): ", Ksp1);
+// for (int i = 0; i < 8; i++) {
+// comm_in[i] = bt.getc();
+// bt.putc(comm_in[i]);
+// }
+// bt.printf("\r\n");
+// Ksp1 = atof(comm_in);
+// servo_ctrl1.setTunings(Ksp1, Ksi1, Ksd1);
+// comm_cmd = 'q';
+// break;
case 5:
- bt.printf("Current: %f, New (8 digits): ", Ksp1);
+ bt.printf("Current: %d, New (8 digits): ", turn_minpwm);
for (int i = 0; i < 8; i++) {
comm_in[i] = bt.getc();
bt.putc(comm_in[i]);
}
bt.printf("\r\n");
- Ksp1 = atof(comm_in);
- servo_ctrl1.setTunings(Ksp1, Ksi1, Ksd1);
+ turn_minpwm = atoi(comm_in);
comm_cmd = 'q';
break;
case 6:
- bt.printf("Current: %d, New (8 digits): ", ctrl_switch);
+ bt.printf("Current: %d, New (8 digits): ", turn_gain);
for (int i = 0; i < 8; i++) {
comm_in[i] = bt.getc();
bt.putc(comm_in[i]);
}
bt.printf("\r\n");
- ctrl_switch = atoi(comm_in);
+ turn_gain = atoi(comm_in);
comm_cmd = 'q';
break;
// case 6:
@@ -290,25 +313,43 @@
// motor_ctrl.setSetPoint(ref_v);
// comm_cmd = 'q';
// break;
- case 7:
+// case 7:
// e_stop = true;
- motor = 1.0;
- bt.printf("STOPPED\r\n");
+// motor = 1.0;
+// bt.printf("STOPPED\r\n");
+// comm_cmd = 'q';
+// break;
+ case 7:
+ bt.printf("Current: %d, New (8 digits): ", turn_thresh);
+ for (int i = 0; i < 8; i++) {
+ comm_in[i] = bt.getc();
+ bt.putc(comm_in[i]);
+ }
+ bt.printf("\r\n");
+ turn_thresh = atoi(comm_in);
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);
+ ref_v = ref_v + 0.25;
+ motor_ctrl.setSetPoint(ref_v);
+// motor_duty = motor_duty + 0.05;
+// ref_pwm = ref_pwm + 0.05;
+// motor = 1.0 - motor_duty;
+// bt.printf("%f\r\n", motor_duty);
+// bt.printf("%f\r\n", ref_pwm);
comm_cmd = 'q';
break;
case 9:
- motor_duty = motor_duty - 0.05;
- motor = 1.0 - motor_duty;
- bt.printf("%f\r\n", motor_duty);
+ ref_v = ref_v - 0.25;
+ motor_ctrl.setSetPoint(ref_v);
+// motor_duty = motor_duty - 0.05;
+// ref_pwm = ref_pwm - 0.05;
+// motor = 1.0 - motor_duty;
+// bt.printf("%f\r\n", motor_duty);
+// bt.printf("%f\r\n", ref_pwm);
comm_cmd = 'q';
break;
}
@@ -424,6 +465,7 @@
// }
servo = angle / 180;
+
// AGC
max = -1;
for (int i = 0; i < 107; i++) {
@@ -449,8 +491,37 @@
dataFlag = 0;
//t_main = t.read_us();
}
+
+ if (motor_ctrl_count == 1000) {
+ velocity = (40000 - bemf_vel) / 3000.0;
+// motor_duty = motor_duty + 0.1;
+// motor = 1.0 - motor_duty;
+ motor_ctrl_count = 0;
+ motor_ctrl.setProcessValue(velocity);
+ motor_duty = motor_ctrl.compute();
+ motor = 1.0 - motor_duty;
+ motor_ctrl_count = 0;
+// }
+// motor_ctrl_count = motor_ctrl_count + 1;
+ } else {
+ motor_ctrl_count = motor_ctrl_count + 1;
+ }
+
+// motor_duty = ref_pwm - ref_pwm * turn_gain * (abs(64 - center) - turn_thresh);
+// if (motor_duty > ref_pwm) {
+// motor_duty = ref_pwm;
+// }
+// if (motor_duty < turn_minpwm) {
+// motor_duty = turn_minpwm;
+// }
+// if (abs(1.0 - motor.read() - motor_duty) > 0.01) {
+// motor = 1.0 - motor_duty;
+// }
+
// t_steer = t.read_us();
+
+
// // Velocity control
// // t.reset();
// if (!e_stop) {
@@ -495,6 +566,14 @@
ctrl_flag = true;
}
+void meas_bemf() {
+ bemf_vel = bemf.read_u16();
+}
+
+void bemf_irq() {
+ bemf_timeout.attach(&meas_bemf, 0.002);
+}
+
// ====
// Main
// ====
@@ -504,17 +583,19 @@
tele_center.set_limits(0, 128);
tele_pwm.set_limits(0.0, 1.0);
+ bemf_int.fall(&bemf_irq);
+
for (int i = 0; i < 10; i++) {
prev_vels[i] = -1;
}
// Initialize motor
- motor.period_us(motor_T);
+ motor.period(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.05, 0.75);
motor_ctrl.setSetPoint(ref_v);
motor_ctrl.setBias(0.0);
motor_ctrl.setMode(1);