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 24:d206fa06c610, committed 2016-04-24
- Comitter:
- vsutardja
- Date:
- Sun Apr 24 23:58:28 2016 +0000
- Parent:
- 23:435bbecc3d23
- Child:
- 25:eef18e3b0515
- Commit message:
- latest
Changed in this revision
--- a/FastAnalogIn.lib Fri Apr 22 19:37:33 2016 +0000 +++ b/FastAnalogIn.lib Sun Apr 24 23:58:28 2016 +0000 @@ -1,1 +1,1 @@ -http://mbed.org/users/Sissors/code/FastAnalogIn/#afc3b84dbbd6 +http://mbed.org/users/Sissors/code/FastAnalogIn/#46fbc645de4d
--- a/PID.lib Fri Apr 22 19:37:33 2016 +0000 +++ b/PID.lib Sun Apr 24 23:58:28 2016 +0000 @@ -1,1 +1,1 @@ -https://developer.mbed.org/teams/EE192-Team-4/code/PID/#f61c47297c2c +https://developer.mbed.org/teams/EE192-Team-4/code/PID/#ee6bcde54713
--- a/main.cpp Fri Apr 22 19:37:33 2016 +0000
+++ b/main.cpp Sun Apr 24 23:58:28 2016 +0000
@@ -56,8 +56,8 @@
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)
+InterruptIn bemf_int(PTD4);
+PwmOut motor(PTD4); // Enable pin (PWM)
Timeout bemf_timeout;
int bemf_vel = 0;
int motor_ctrl_count = 0;
@@ -80,7 +80,9 @@
float Kmd = 0; // Derivative factor
float interval = 0.01; // Sampling interval
float prev_vels[10];
-float ref_v = 1.5; // Reference velocity
+float ref_v = 0; // Reference velocity
+float master_v = 0;
+float turn_minv = 1.0;
PID motor_ctrl(Kmp, Kmi, Kmd, interrupt_T); // Motor controller
int turn_thresh = 15;
@@ -93,9 +95,9 @@
// Servo
// =====
float angle = 88; // Angle
-float Ksp = 0.9; // Steering proportion
-float Ksi = 0;
-float Ksd = 0;
+float Ksp = 0.8; // Steering proportion
+float Ksi = 10000000;
+float Ksd = 0.0000001;
PID servo_ctrl(Ksp, Ksi, Ksd, interrupt_T);
float Ksp1 = 0.9;
float Ksi1 = 0;
@@ -117,8 +119,8 @@
const int T_INT_MAX = interrupt_T * 1000000 - 1000; // Maximum exposure time
const int T_INT_MIN = 35; // Minimum exposure time
int img[108] = {0}; // Image data
-DigitalOut clk(PTD5); // Clock pin
-DigitalOut si(PTD0); // SI pin
+DigitalOut clk(PTD0); // Clock pin
+DigitalOut si(PTD5); // SI pin
FastAnalogIn ao(PTC2); // AO pin
// ================
@@ -183,14 +185,14 @@
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 turn slowing minimum pwm\r\n");
+ bt.printf(" [5] Change turn slowing minimum velocity\r\n");
// bt.printf(" [6] Change reference velocity\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] ref_pwm += 0.05\r\n");
- bt.printf(" [9] ref_pwm -= 0.05\r\n");
+ bt.printf(" [8] master_v += 0.05\r\n");
+ bt.printf(" [9] master_v -= 0.05\r\n");
comm_cmd = bt.getc();
while (comm_cmd != 'q') {
t.reset();
@@ -283,13 +285,13 @@
// comm_cmd = 'q';
// break;
case 5:
- bt.printf("Current: %d, New (8 digits): ", turn_minpwm);
+ bt.printf("Current: %d, New (8 digits): ", turn_minv);
for (int i = 0; i < 8; i++) {
comm_in[i] = bt.getc();
bt.putc(comm_in[i]);
}
bt.printf("\r\n");
- turn_minpwm = atoi(comm_in);
+ turn_minv = atoi(comm_in);
comm_cmd = 'q';
break;
case 6:
@@ -333,23 +335,25 @@
// 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:
- ref_v = ref_v + 0.25;
- motor_ctrl.setSetPoint(ref_v);
+ master_v = master_v + 0.25;
+ motor_ctrl.setSetPoint(master_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);
+ bt.printf("%f\r\n", master_v);
comm_cmd = 'q';
break;
case 9:
- ref_v = ref_v - 0.25;
- motor_ctrl.setSetPoint(ref_v);
+ master_v = master_v - 0.25;
+ motor_ctrl.setSetPoint(master_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);
+ bt.printf("%f\r\n", master_v);
comm_cmd = 'q';
break;
}
@@ -366,18 +370,18 @@
debug = !debug;
}
void cam_Read(){
- PTD->PCOR = (0x01 << 5);
+ PTD->PCOR = (0x01);
+ PTD->PSOR = (0x01 << 5);
PTD->PSOR = (0x01);
- PTD->PSOR = (0x01 << 5);
- PTD->PCOR = (0x01);
+ PTD->PCOR = (0x01 << 5);
for (int i = 0; i < 128; i++) {
- PTD->PCOR = (0x01 << 5);
+ PTD->PCOR = (0x01);
if (i > 9 && i < 118) {
img[i-10] = ao.read_u16();
tele_linescan[i-10] = img[i-10];
}
- PTD->PSOR = (0x01 << 5);
+ PTD->PSOR = (0x01);
}
dataFlag = 1;
debugger.attach(bugger,0.2);
@@ -392,14 +396,14 @@
debug2 = 1;
// Dummy read
if (rdyFlag){
- PTD->PCOR = (0x01 << 5);
+ PTD->PCOR = (0x01);
+ PTD->PSOR = (0x01 << 5);
PTD->PSOR = (0x01);
- PTD->PSOR = (0x01 << 5);
- PTD->PCOR = (0x01);
+ PTD->PCOR = (0x01 << 5);
for (int i = 0; i < 128; i++) {
- PTD->PCOR = (0x01 << 5);
- PTD->PSOR = (0x01 << 5);
+ PTD->PCOR = (0x01);
+ PTD->PSOR = (0x01);
}
// Expose
@@ -493,13 +497,13 @@
}
if (motor_ctrl_count == 1000) {
- velocity = (40000 - bemf_vel) / 3000.0;
+ velocity = (40000 - bemf_vel) / 6000.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 = motor_duty;
motor_ctrl_count = 0;
// }
// motor_ctrl_count = motor_ctrl_count + 1;
@@ -507,6 +511,17 @@
motor_ctrl_count = motor_ctrl_count + 1;
}
+ ref_v = master_v - master_v * turn_gain * (abs(64 - center) - turn_thresh);
+ if (ref_v < turn_minv) {
+ ref_v = turn_minv;
+ }
+ if (ref_v > master_v) {
+ ref_v = master_v;
+ }
+ if (abs(motor_ctrl.getSetPoint() - ref_v) > 0.05) {
+ motor_ctrl.setSetPoint(ref_v);
+ }
+
// motor_duty = ref_pwm - ref_pwm * turn_gain * (abs(64 - center) - turn_thresh);
// if (motor_duty > ref_pwm) {
// motor_duty = ref_pwm;
@@ -591,12 +606,12 @@
// Initialize motor
motor.period(motor_T);
- motor = 1.0 - motor_duty;
+ motor = motor_duty;
// Initialize motor controller
motor_ctrl.setInputLimits(0.0, 10.0);
motor_ctrl.setOutputLimits(0.05, 0.75);
- motor_ctrl.setSetPoint(ref_v);
+ motor_ctrl.setSetPoint(master_v);
motor_ctrl.setBias(0.0);
motor_ctrl.setMode(1);