Rick Poppe
naam van een functie veranderd
Dependencies: FastPWM HIDScope MODSERIAL QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 5:0581d843fde2
- Parent:
- 4:b83460036800
--- a/main.cpp Sat Nov 05 16:40:39 2016 +0000
+++ b/main.cpp Sat Nov 05 17:27:31 2016 +0000
@@ -8,7 +8,7 @@
// In use: D(0(TX),1(RX),4(motor2dir),5(motor2pwm),6(motor1pwm),7(motor1dir),
// 8(pushbutton),9(servoPWM),10(encoder),11(encoder),12(encoder),13(encoder)) A(0,1,2)(EMG)
-MODSERIAL pc(USBTX, USBRX);
+MODSERIAL pc(USBTX, USBRX);
// Define the EMG inputs
AnalogIn emg_in1( A0 );
@@ -16,72 +16,60 @@
AnalogIn emg_in3( A2 );
// Define motor outputs
-DigitalOut motor1dir(D7); // Direction of motor 1, attach at m1, set to 0: cw
-DigitalOut motor2dir(D4); // Direction of motor 2, attach at m2, set to 0: ccw
-FastPWM motor1(D6); // Speed of motor 1
-FastPWM motor2(D5); // Speed of motor 2
-FastPWM servo(D9); // Servo pwm
+DigitalOut motor1dir(D7); // Direction of motor 1, attach at m1, set to 0: cw
+DigitalOut motor2dir(D4); // Direction of motor 2, attach at m2, set to 0: ccw
+FastPWM motor1(D6); // Speed of motor 1
+FastPWM motor2(D5); // Speed of motor 2
+FastPWM servo(D9); // Servo pwm
-// Define servo input
-DigitalIn servo_button(PTC12);
+// Define button for flipping the spatula
+DigitalIn servo_button(PTC12);
// Define encoder inputs
-QEI Encoder1(D13,D12,NC,64,QEI::X4_ENCODING);
-QEI Encoder2(D11,D10,NC,64,QEI::X4_ENCODING);
+QEI encoder1(D13,D12,NC,64,QEI::X4_ENCODING);
+QEI encoder2(D11,D10,NC,64,QEI::X4_ENCODING);
// Define the Tickers
Ticker print_timer; // Ticker for printing the position or highest EMG values
-Ticker sample_timer; // Ticker for when a sample needs to be taken
-Ticker control_timer; // Ticker for processing encoder input to motor output
+Ticker controller_timer; // Ticker for when a sample needs to be taken and the motor need to be controlled
Ticker servo_timer; // Ticker for calling servo_control
// Define the Time constants
-const double freq = 0.002; // EMG sample time
-const double m1_Ts = 0.002; // Controller sample time
-const double m2_Ts = 0.002; // Controller sample time
-const double servo_Ts = 0.02; // Servo controller sample time
+const double Ts = 0.002; // Time constant for activate_controller()
+const double servo_Ts = 0.02; // Time constant for activate_servo_controller()
// Define the go flags
-volatile bool change_ref_go = false; // Go flag sample()
-volatile bool controller_go = false; // Go flag controller()
-volatile bool servo_go = false; // Go flag servo_controller()
+volatile bool change_controller_go = false; // Go flag for sample() and motor_controller()
+volatile bool servo_go = false; // Go flag servo_controller()
// Define the EMG variables
-double emg1; // The first EMG signal
-double emg2; // The second EMG signal
-double emg3; // The third EMG signal
-double highest_emg1; // The highest EMG signal of emg_in1
-double highest_emg2; // The highest EMG signal of emg_in2
-double highest_emg3; // The highest EMG signal of emg_in3
-double threshold1; // The threshold which the first EMG signal needs to surpass to do something
-double threshold2; // The threshold which the second EMG signal needs to surpass to do something
-double threshold3; // The threshold which the third EMG signal needs to surpass to do something
+double emg1, emg2, emg3; // The three filtered EMG signals
+double highest_emg1, highest_emg2, highest_emg3; // The highest EMG signals of emg_in
+double threshold1, threshold2, threshold3; // The threshold which the EMG signals need to surpass to change the reference
//Define the keyboard input
char key; // Stores the last pressed key on the keyboard
// Define the reference variables
-double ref_x = 0.0; // The x reference position
-double ref_y = 0.0; // The y reference position
-double old_ref_x; // The old x reference
-double old_ref_y; // The old y reference
+double ref_x = 0.0, ref_y = 0.0; // The reference position
+double old_ref_x, old_ref_y; // The old reference position
double speed = 0.00006; // The variable with which a speed is reached of 3 cm/s in x and y direction
double theta = 0.0; // The angle reference of the arm
double radius = 0.0; // The radius reference of the arm
bool z_pushed = false; // To see if z is pressed
// Define reference limits
-const double min_radius=0.43; // The minimum radius of arm
-const double max_radius=0.62; // The maximum radius of arm
-const double min_y=-0.26; // The minimum height which the spatula can reach
+const double min_radius = 0.43; // The minimum radius of the arm
+const double max_radius = 0.62; // The maximum radius of the arm
+const double min_y = -0.26; // The minimum height which the spatula can reach
// Define variables of motor 1
-double m1_pwm=0; // Variable for PWM control motor 1
+double m1_pwm = 0; // Variable for PWM control motor 1
const double m1_Kp = 35.16, m1_Ki = 108.8, m1_Kd = 2.84, m1_N = 100; // PID values of motor 1
double m1_v1 = 0, m1_v2 = 0; // Memory variables
// Define variables of motor 2
-double m2_pwm=0; // Variable for PWM control motor 2
+double m2_pwm = 0; // Variable for PWM control motor 2
const double m2_Kp = 36.24, m2_Ki = 108.41, m2_Kd = 3.03, m2_N = 100; // PID values of motor 2
double m2_v1 = 0, m2_v2 = 0; // Memory variables
@@ -144,158 +132,148 @@
// Low pass filter
BiQuad bq331 = bq131;
-void activate_sample() // Go flag for the function sample()
+void activate_controller() // Go flag for the functions sample() and controller()
{
- if (change_ref_go==true) {
+ if (change_controller_go == true) {
// This if statement is used to see if the code takes too long before it is called again
- pc.printf("rate too high error in activate_sample\n\r");
+ pc.printf("rate too high, error in activate_controller\n\r");
}
- change_ref_go=true; // Activate go flag
-}
-
-void activate_controller() // Go flag for the function activate_controller()
-{
- if (controller_go==true) {
- // This if statement is used to see if the code takes too long before it is called again
- pc.printf("rate too high error in activate_controller()\n\r");
- }
- controller_go=true; // Activate go flag
+ change_controller_go = true; // Activate go flag
}
void activate_servo_control() // Go flag for the function servo_controller()
{
- if (servo_go==true) {
+ if (servo_go == true) {
// This if statement is used to see if the code takes too long before it is called again
- pc.printf("rate too high error in servo_controller()\n\r");
+ pc.printf("rate too high, error in servo_controller()\n\r");
}
- servo_go=true; // Activate go flag
+ servo_go = true; // Activate go flag
}
-void change_ref() // Function for sampling of the emg signal and changing the reference position
+void sample() // Function for sampling of the emg signal and changing the reference position
{
// Change key if the keyboard is pressed
- if (pc.readable()==1) {
+ if (pc.readable() == 1) {
key=pc.getc();
}
// Read the emg signals and filter it
- emg1=bqc12.step(fabs(bqc11.step(emg_in1.read()))); //filtered signal 1
- emg2=bqc22.step(fabs(bqc21.step(emg_in2.read()))); //filtered signal 2
- emg3=bqc32.step(fabs(bqc31.step(emg_in3.read()))); //filtered signal 3
+ emg1 = bqc12.step(fabs(bqc11.step(emg_in1.read()))); //filtered signal 1
+ emg2 = bqc22.step(fabs(bqc21.step(emg_in2.read()))); //filtered signal 2
+ emg3 = bqc32.step(fabs(bqc31.step(emg_in3.read()))); //filtered signal 3
// Remember what the old reference was
- old_ref_x=ref_x;
- old_ref_y=ref_y;
+ old_ref_x = ref_x;
+ old_ref_y = ref_y;
// Change the reference if the emg signals go over the threshold
- if (emg1>threshold1&&emg2>threshold2&&emg3>threshold3 || key=='d') // Negative XY direction
+ if (emg1 > threshold1 && emg2 > threshold2 && emg3 > threshold3 || key == 'd') // Negative XY direction
{
- ref_x=ref_x-speed;
- ref_y=ref_y-speed;
+ ref_x = ref_x - speed;
+ ref_y = ref_y - speed;
- } else if (emg1>threshold1&&emg2>threshold2 || key == 'a' || key == 'z') // Negative X direction
+ } else if (emg1 > threshold1 && emg2 > threshold2 || key == 'a' || key == 'z') // Negative X direction
{
- ref_x=ref_x-speed;
+ ref_x = ref_x - speed;
- } else if (emg1>threshold1&&emg3>threshold3 || key == 's') // Negative Y direction
+ } else if (emg1 > threshold1 && emg3 > threshold3 || key == 's') // Negative Y direction
{
- ref_y=ref_y-speed;
+ ref_y = ref_y - speed;
- } else if (emg2>threshold2&&emg3>threshold3 || key == 'e' ) // Positive XY direction
+ } else if (emg2 > threshold2 && emg3 > threshold3 || key == 'e' ) // Positive XY direction
{
- ref_x=ref_x+speed;
- ref_y=ref_y+speed;
+ ref_x = ref_x + speed;
+ ref_y = ref_y + speed;
- } else if (emg2>threshold2 || key == 'q' ) // Positive X direction
+ } else if (emg2 > threshold2 || key == 'q' ) // Positive X direction
{
- ref_x=ref_x+speed;
+ ref_x = ref_x + speed;
- } else if (emg3>threshold3 || key == 'w') // Positive Y direction
+ } else if (emg3 > threshold3 || key == 'w') // Positive Y direction
{
- ref_y=ref_y+speed;
+ ref_y = ref_y + speed;
}
// Change z_pushed to true if 'z' is pressed on the keyboard
if (key == 'z') {
- z_pushed=true;
+ z_pushed = true;
}
// Reset the reference and the encoders if z is no longer pressed
if (key != 'z' && z_pushed) {
- ref_x=0.0;
- ref_y=0.0;
- Encoder1.reset();
- Encoder2.reset();
- z_pushed=false;
+ ref_x = 0.0;
+ ref_y = 0.0;
+ encoder1.reset();
+ encoder2.reset();
+ z_pushed = false;
}
// Convert the x and y reference to the theta and radius reference
- theta=atan(ref_y/(ref_x+min_radius));
- radius=sqrt(pow(ref_x+min_radius,2)+pow(ref_y,2));
+ theta = atan( ref_y / (ref_x + min_radius));
+ radius = sqrt( pow( ref_x + min_radius, 2) + pow( ref_y, 2));
// If the new reference is outside the possible range then revert back to the old reference unless z is pressed
if (radius < min_radius) {
if (key != 'z') {
- ref_x=old_ref_x;
- ref_y=old_ref_y;
+ ref_x = old_ref_x;
+ ref_y = old_ref_y;
}
} else if ( radius > max_radius) {
- ref_x=old_ref_x;
- ref_y=old_ref_y;
- } else if (ref_y<min_y) {
- ref_y=old_ref_y;
+ ref_x = old_ref_x;
+ ref_y = old_ref_y;
+ } else if (ref_y < min_y) {
+ ref_y = old_ref_y;
}
// Calculate theta and radius again
- theta=atan(ref_y/(ref_x+min_radius));
- radius=sqrt(pow(ref_x+min_radius,2)+pow(ref_y,2));
-
+ theta = atan( ref_y / (ref_x + min_radius));
+ radius = sqrt( pow( ref_x + min_radius, 2) + pow( ref_y, 2));
}
double PID( double err, const double Kp, const double Ki, const double Kd,
const double Ts, const double N, double &v1, double &v2 ) //discrete PIDF filter
{
- const double a1 =-4/(N*Ts+2),
- a2=-(N*Ts-2)/(N*Ts+2),
- b0=(4*Kp + 4*Kd*N + 2*Ki*Ts+2*Kp*N*Ts+Ki*N*pow(Ts,2))/(2*N*Ts+4),
- b1=(Ki*N*pow(Ts,2)-4*Kp-4*Kd*N)/(N*Ts+2),
- b2=(4*Kp+4*Kd*N-2*Ki*Ts-2*Kp*N*Ts+Ki*N*pow(Ts,2))/(2*N*Ts+4);
+ const double a1 = -4 / (N * Ts + 2),
+ a2 = -(N * Ts - 2)/(N*Ts + 2),
+ b0 = (4 * Kp + 4 * Kd * N + 2 * Ki * Ts + 2 * Kp * N * Ts + Ki * N * pow(Ts, 2)) / (2 * N * Ts + 4),
+ b1 = (Ki * N * pow(Ts, 2) - 4 * Kp - 4 * Kd * N) / (N * Ts + 2),
+ b2 = (4 * Kp + 4 * Kd * N - 2 * Ki * Ts - 2 * Kp * N * Ts + Ki * N * pow(Ts, 2)) / (2 * N * Ts + 4);
- double v=err-a1*v1-a2*v2;
- double u=b0*v+b1*v1+b2*v2;
- v2=v1;
- v1=v;
+ double v = err - a1 * v1 - a2 * v2;
+ double u = b0 * v + b1 * v1 + b2 * v2;
+ v2 = v1;
+ v1 = v;
return u;
}
-void controller() // Function for executing controller action
+void motor_controller() // Function for executing controller action
{
// Convert radius and theta to gearbox angles
- double ref_angle1=16*theta;
- double ref_angle2=(-radius+min_radius)/pulley_radius;
+ double ref_angle1 = 16 * theta;
+ double ref_angle2 = (-radius + min_radius) / pulley_radius;
// Get number of pulses of the encoders
- double angle1 = Encoder1.getPulses()/res; //counterclockwise is positive
- double angle2 = Encoder2.getPulses()/res;
+ double angle1 = encoder1.getPulses() / res; //counterclockwise is positive
+ double angle2 = encoder2.getPulses() / res;
// Calculate the motor pwm using the function PID() and the voltage
- m1_pwm = (PID(ref_angle1-angle1,m1_Kp,m1_Ki,m1_Kd,m1_Ts,m1_N,m1_v1,m1_v2))/V_max;
- m2_pwm = (PID(ref_angle2-angle2,m2_Kp,m2_Ki,m2_Kd,m2_Ts,m2_N,m2_v1,m2_v2))/V_max;
+ m1_pwm = (PID(ref_angle1 - angle1, m1_Kp, m1_Ki, m1_Kd, Ts, m1_N, m1_v1, m1_v2)) / V_max;
+ m2_pwm = (PID(ref_angle2 - angle2, m2_Kp, m2_Ki, m2_Kd, Ts, m2_N, m2_v1, m2_v2)) / V_max;
// Limit pwm value and change motor direction when pwm becomes either negative or positive
- if (m1_pwm >=0.0f && m1_pwm <=1.0f) {
- motor1dir=0;
+ if (m1_pwm >= 0.0f && m1_pwm <= 1.0f) {
+ motor1dir = 0;
motor1.write(m1_pwm);
} else if (m1_pwm < 0.0f && m1_pwm >= -1.0f) {
- motor1dir=1;
+ motor1dir = 1;
motor1.write(-m1_pwm);
}
- if (m2_pwm >=0.0f && m2_pwm <=1.0f) {
- motor2dir=0;
+ if (m2_pwm >= 0.0f && m2_pwm <= 1.0f) {
+ motor2dir = 0;
motor2.write(m2_pwm);
} else if (m2_pwm < 0.0f && m2_pwm >= -1.0f) {
- motor2dir=1;
+ motor2dir = 1;
motor2.write(-m2_pwm);
}
}
@@ -304,19 +282,18 @@
{
// If theta is smaller than max_theta then the servo_pwm is adjusted to stabilize the spatula
if (theta < max_theta ) { // servo can stabilize until maximum theta
- servo_pwm=min_servo_pwm+(theta-min_theta)/diff_theta*res_servo;
+ servo_pwm = min_servo_pwm + (theta - min_theta) / diff_theta * res_servo;
} else {
- servo_pwm=max_servo_pwm;
+ servo_pwm = max_servo_pwm;
}
// The spatula goes to its maximum position to flip a burger if the button is pressed
if (!servo_button) {
- servo_pwm=0.0014;
+ servo_pwm = 0.0014;
}
// Send the servo_pwm to the servo
servo.pulsewidth(servo_pwm);
-
}
void my_emg() // This function prints the highest emg values to putty
@@ -350,64 +327,56 @@
motor2.period(0.02f);
// Set the direction of the motors to ccw
- motor1dir=0;
- motor2dir=0;
+ motor1dir = 0;
+ motor2dir = 0;
// Attaching the function my_emg() to the ticker print_timer
print_timer.attach(&my_emg, 1);
// While loop used for calibrating the emg thresholds, So that every user can use it
- while (servo_button==1) {
- emg1=bqc12.step(fabs(bqc11.step(emg_in1.read()))); //filtered signal 1
- emg2=bqc22.step(fabs(bqc21.step(emg_in2.read()))); //filtered signal 2
- emg3=bqc32.step(fabs(bqc31.step(emg_in3.read()))); //filtered signal 3
+ while (servo_button == 1) {
+ emg1 = bqc12.step(fabs(bqc11.step(emg_in1.read()))); //filtered signal 1
+ emg2 = bqc22.step(fabs(bqc21.step(emg_in2.read()))); //filtered signal 2
+ emg3 = bqc32.step(fabs(bqc31.step(emg_in3.read()))); //filtered signal 3
// Check if the new EMG signal is higher than the current highest value
- if(emg1>highest_emg1) {
- highest_emg1=emg1;
+ if(emg1 > highest_emg1) {
+ highest_emg1 = emg1;
}
- if(emg2>highest_emg2) {
- highest_emg2=emg2;
+ if(emg2 > highest_emg2) {
+ highest_emg2 = emg2;
}
- if(emg3>highest_emg3) {
- highest_emg3=emg3;
+ if(emg3 > highest_emg3) {
+ highest_emg3 = emg3;
}
- // Define the thresholds as 0.3 the highest emg value
- threshold1=0.30*highest_emg1;
- threshold2=0.30*highest_emg2;
- threshold3=0.30*highest_emg3;
+ // Define the thresholds as 0.3 times the highest emg value
+ threshold1 = 0.30 * highest_emg1;
+ threshold2 = 0.30 * highest_emg2;
+ threshold3 = 0.30 * highest_emg3;
}
- // Attach the function sample() to the ticker sample_timer
- sample_timer.attach(&activate_sample, freq);
+ // Attach the function activate_controller() to the ticker change_pos_timer
+ controller_timer.attach(&activate_controller, Ts);
// Attach the function my_pos() to the ticker print_timer.
print_timer.attach(&my_pos, 1);
- // Attach the function activate_controller() to the ticker control_timer
- control_timer.attach(&activate_controller,m1_Ts);
-
// Attach the function activate_servo_control() to the ticker servo_timer
servo_timer.attach(&activate_servo_control,servo_Ts);
while(1) {
- // Only take a sample when the go flag is true.
- if (change_ref_go==true) {
- change_ref();
- change_ref_go = false;
+ // Take a sample and control the motor when the go flag is true.
+ if (change_controller_go == true) {
+ sample();
+ motor_controller();
+ change_controller_go = false;
}
- // Only control the motor when the go flag is true
- if(controller_go) {
- controller();
- controller_go=false;
- }
-
- // Only control the servo when the go flag is true
- if(servo_go) {
+ // Control the servo when the go flag is true
+ if(servo_go == true) {
servo_controller();
servo_go=false;
}