Rick Poppe
naam van een functie veranderd
Dependencies: FastPWM HIDScope MODSERIAL QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 4:b83460036800
- Parent:
- 3:58378b78079d
- Child:
- 5:0581d843fde2
--- a/main.cpp Tue Nov 01 15:00:40 2016 +0000
+++ b/main.cpp Sat Nov 05 16:40:39 2016 +0000
@@ -5,244 +5,255 @@
#include "QEI.h"
#include "FastPWM.h"
-// in gebruik: 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)
+// 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);
-HIDScope scope(6); // the amount of scopes to send to the pc
-//Define objects
+// Define the EMG inputs
+AnalogIn emg_in1( A0 );
+AnalogIn emg_in2( A1 );
+AnalogIn emg_in3( A2 );
-//Define the EMG inputs
-AnalogIn emg1( A0 );
-AnalogIn emg2( A1 );
-AnalogIn emg3( 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
+
+// Define servo input
+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);
-//Define motor outputs
-DigitalOut motor1dir(D7); //direction of motor 1, attach at m1, set to 0: cw
-FastPWM motor1(D6); // speed of motor 1
-FastPWM motor2(D5); //speed of motor 2
-DigitalOut motor2dir(D4); //direction of motor 2, attach at m2, set to 0: ccw
-FastPWM servo(D9); //servo pwm
-DigitalIn servo_button(PTC12); // servo flip
+// 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 servo_timer; // Ticker for calling servo_control
-QEI Encoder1(D13,D12,NC,64,QEI::X4_ENCODING); //defining encoder
-QEI Encoder2(D11,D10,NC,64,QEI::X4_ENCODING); //defining encoder
+// 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
-//Define the Tickers
-Ticker pos_timer; // the timer which is used to print the position every second
-Ticker sample_timer; // the timer which is used to decide when a sample needs to be taken
-Ticker control; // Ticker for processing encoder input to motor output
-Ticker servo_control; // Ticker for calling servo_control
+// 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()
-//Initialize all variables
-volatile bool sample_go = false; // go flag sample()
-volatile bool controller_go=false; // go flag 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
+//Define the keyboard input
+char key; // Stores the last pressed key on the keyboard
-double highest_emg1; // the highest emg signal of emg input 1
-double highest_emg2; // the highest emg signal of emg input 2
-double highest_emg3; // the highest emg signal of emg input 3
-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 samplefreq=0.002; // every 0.002 sec a sample will be taken this is a frequency of 500 Hz
-double emg11; // the first emg signal
-double emg21; // the second emg signal
-double emg31; // the third emg signal
-double ref_x=0.0000; // the x reference position
-double ref_y=0.0000; // the y reference position
-double old_ref_x; // the old x reference
-double old_ref_y; // the old y reference
-double speed=0.00008; // the variable with which a speed is reached of 1cm/s
-double theta=0.0; // angle of the arm
-double radius=0.0; // radius of the arm
+// 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 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
-const double minRadius=0.43; // minimum radius of arm
-const double maxRadius=0.62; // maximum radius of arm
-const double min_y=-0.26; // minimum height which the spatula can reach
-char key; // variable to place the keyboard input
+// 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
-double m1_pwm=0; //variable for PWM control motor 1
-double m2_pwm=0; //variable for PWM control motor 2
+// Define variables of 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
-const double m1_Kp = 35.16, m1_Ki = 108.8, m1_Kd = 2.84, m1_N = 100; // controller constants motor 1
-double m1_v1 = 0, m1_v2 = 0; // Memory variables
-const double m1_Ts = 0.01; // Controller sample time
+// Define variables of 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
-const double m2_Kp = 36.24, m2_Ki = 108.41, m2_Kd = 3.03, m2_N = 100; // controller constants motor 2
-double m2_v1 = 0, m2_v2 = 0; // Memory variables
-const double m2_Ts = 0.01; // Controller sample time
-
-const double pi=3.14159265359;
-const double res = 64.0/(1.0/131.25*2.0*pi); // resolution on gearbox shaft per pulse
-const double V_max=9.0; // maximum voltage supplied by trafo
-const double pulleyRadius=0.0398/2.0; // pulley radius
+// Define machine constants
+const double pi = 3.14159265359;
+const double res = 64.0 / (1.0 / 131.25 * 2.0 * pi); // Resolution on gearbox shaft per pulse
+const double V_max = 9.0; // Maximum voltage supplied by trafo
+const double pulley_radius = 0.0398/2.0; // Pulley radius
-double servo_pwm=0.0023; // duty cycle 1.5 ms 7.5%, min 0.5 ms 2.5%, max 2.5 ms 12.5%
-const double minTheta=-38.0/180.0*pi; //minimum angle robot
-const double maxTheta=-24.0/180.0*pi; // maximum angle to which the spatula can stabilise
-const double diffTheta=maxTheta-minTheta; //difference between max and min angle of theta for stabilisation
-const double min_servo_pwm=0.0020; // corresponds to angle of theta -38 degrees
-const double max_servo_pwm=0.0022; // corresponds to angle of theta -24 degrees
-const double res_servo=max_servo_pwm-min_servo_pwm; //resolution of servo pwm signal between min and max angle
-const double servo_Ts=0.02;
-bool z_push=false;
+// Define variables needed for controlling the servo
+double servo_pwm = 0.0023; // Duty cycle 1.5 ms 7.5%, min 0.5 ms 2.5%, max 2.5 ms 12.5%
+const double min_theta = -37.0 / 180.0 * pi; // Minimum angle robot
+const double max_theta = -14.0 / 180.0 * pi; // Maximum angle to which the spatula can stabilise
+const double diff_theta = max_theta - min_theta; // Difference between max and min angle of theta for stabilisation
+const double min_servo_pwm = 0.0021; // Corresponds to angle of theta -38 degrees
+const double max_servo_pwm = 0.0024; // Corresponds to angle of theta -24 degrees
+const double res_servo = max_servo_pwm - min_servo_pwm; // Resolution of servo pwm signal between min and max angle
-//Define the needed Biquad chains
+// Define the Biquad chains
BiQuadChain bqc11;
-BiQuadChain bqc13;
+BiQuadChain bqc12;
BiQuadChain bqc21;
-BiQuadChain bqc23;
+BiQuadChain bqc22;
BiQuadChain bqc31;
-BiQuadChain bqc33;
+BiQuadChain bqc32;
-//Define the BiQuads for the filter of the first emg signal
-//Notch filter
+// Define the BiQuads for the filter of the first emg signal
+// Notch filter
BiQuad bq111(0.9795, -1.5849, 0.9795, 1.0000, -1.5849, 0.9589);
BiQuad bq112(0.9833, -1.5912, 0.9833, 1.0000, -1.5793, 0.9787);
BiQuad bq113(0.9957, -1.6111, 0.9957, 1.0000, -1.6224, 0.9798);
-//High pass filter
-//BiQuad bq121( 9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01 ); //Old biquad values
+// High pass filter
BiQuad bq121( 0.8956, -1.7911, 0.8956, 1.0000, -1.7814, 0.7941);
BiQuad bq122( 0.9192, -1.8385, 0.9192, 1.0000, -1.8319, 0.8450);
BiQuad bq123( 0.9649, -1.9298, 0.9649, 1.0000, -1.9266, 0.9403);
-//Low pass filter
+// Low pass filter
BiQuad bq131( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
-//Define the BiQuads for the filter of the second emg signal
-//Notch filter
+// Define the BiQuads for the filter of the second emg signal
+// Notch filter
BiQuad bq211 = bq111;
BiQuad bq212 = bq112;
BiQuad bq213 = bq113;
-//High pass filter
+// High pass filter
BiQuad bq221 = bq121;
BiQuad bq222 = bq122;
BiQuad bq223 = bq123;
-//Low pass filter
+// Low pass filter
BiQuad bq231 = bq131;
-//Define the BiQuads for the filter of the third emg signal
-//notch filter
+// Define the BiQuads for the filter of the third emg signal
+// Notch filter
BiQuad bq311 = bq111;
BiQuad bq312 = bq112;
BiQuad bq313 = bq113;
-//High pass filter
+// High pass filter
BiQuad bq321 = bq121;
BiQuad bq323 = bq122;
BiQuad bq322 = bq123;
-//low pass filter
+// Low pass filter
BiQuad bq331 = bq131;
-void activate_sample()
+void activate_sample() // Go flag for the function sample()
{
- if (sample_go==true) {
- // this if statement is used to see if the code takes too long before it is called again
+ if (change_ref_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");
}
- //This sets the go flag for when the function sample needs to be called
- sample_go=true;
+ change_ref_go=true; // Activate go flag
}
-void activate_controller()
+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
+ // 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
+ controller_go=true; // Activate go flag
}
-void activate_servo_control()
+void activate_servo_control() // Go flag for the function servo_controller()
{
if (servo_go==true) {
- pc.printf("error servo");
+ // 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");
}
- servo_go=true; //activate go flag
+ servo_go=true; // Activate go flag
}
-void sample()
+void change_ref() // Function for sampling of the emg signal and changing the reference position
{
- //This checks if a key is pressed and changes the variable key in the pressed key
+ // Change key if the keyboard is pressed
if (pc.readable()==1) {
key=pc.getc();
}
- //Read the emg signals and filter it
+
+ // 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
- //scope.set(0,emg1.read());
- emg11=bqc13.step(fabs(bqc11.step(emg1.read()))); //filtered signal 1
- //scope.set(1,emg11);
-
- //scope.set(2,emg2.read());
- emg21=bqc23.step(fabs(bqc21.step(emg2.read()))); //filtered signal 2
- //scope.set(3,emg21);
-
- //scope.set(4,emg3.read());
- emg31=bqc33.step(fabs(bqc31.step(emg3.read()))); //filtered signal 3
- //scope.set(5,emg31);
-
- //remember what the reference was
+ // Remember what the old reference was
old_ref_x=ref_x;
old_ref_y=ref_y;
- //look if the emg signals go over the threshold and change the reference accordingly
- if (emg11>threshold1&&emg21>threshold2&&emg31>threshold3 || key=='d') {
+
+ // Change the reference if the emg signals go over the threshold
+ if (emg1>threshold1&&emg2>threshold2&&emg3>threshold3 || key=='d') // Negative XY direction
+ {
ref_x=ref_x-speed;
ref_y=ref_y-speed;
- } else if (emg11>threshold1&&emg21>threshold2 || key == 'a' || key == 'z') {
+ } else if (emg1>threshold1&&emg2>threshold2 || key == 'a' || key == 'z') // Negative X direction
+ {
ref_x=ref_x-speed;
-
- } else if (emg11>threshold1&&emg31>threshold3 || key == 's') {
+
+ } else if (emg1>threshold1&&emg3>threshold3 || key == 's') // Negative Y direction
+ {
ref_y=ref_y-speed;
- } else if (emg21>threshold2&&emg31>threshold3 || key == 'e' ) {
+ } else if (emg2>threshold2&&emg3>threshold3 || key == 'e' ) // Positive XY direction
+ {
ref_x=ref_x+speed;
ref_y=ref_y+speed;
- } else if (emg21>threshold2 || key == 'q' ) {
+ } else if (emg2>threshold2 || key == 'q' ) // Positive X direction
+ {
ref_x=ref_x+speed;
- } else if (emg31>threshold3 || key == 'w') {
+ } else if (emg3>threshold3 || key == 'w') // Positive Y direction
+ {
ref_y=ref_y+speed;
}
+ // Change z_pushed to true if 'z' is pressed on the keyboard
if (key == 'z') {
- z_push=true;
+ z_pushed=true;
}
- if (key != 'z' && z_push) {
+ // 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_push=false;
+ z_pushed=false;
}
- // convert the x and y reference to the theta and radius reference
- theta=atan(ref_y/(ref_x+minRadius));
- radius=sqrt(pow(ref_x+minRadius,2)+pow(ref_y,2));
+ // 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));
- //look if the new reference is outside the possible range and revert back to the old reference if it is outside the range
- if (radius < minRadius) {
+ // 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;
}
- } else if ( radius > maxRadius) {
+ } 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;
}
- theta=atan(ref_y/(ref_x+minRadius));
- radius=sqrt(pow(ref_x+minRadius,2)+pow(ref_y,2));
+
+ // 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));
- //scope.send();
}
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 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),
@@ -257,20 +268,21 @@
return u;
}
-void controller() //function for executing controller action
+void controller() // Function for executing controller action
{
-
- //converting radius and theta to gearbox angle
+ // Convert radius and theta to gearbox angles
double ref_angle1=16*theta;
- double ref_angle2=(-radius+minRadius)/pulleyRadius;
+ double ref_angle2=(-radius+min_radius)/pulley_radius;
- double angle1 = Encoder1.getPulses()/res; //get number of pulses (counterclockwise is positive)
- double angle2 = Encoder2.getPulses()/res; //get number of pulses
+ // Get number of pulses of the encoders
+ 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;
- //divide by voltage to get pwm duty cycle percentage)
m2_pwm = (PID(ref_angle2-angle2,m2_Kp,m2_Ki,m2_Kd,m2_Ts,m2_N,m2_v1,m2_v2))/V_max;
- //limit pwm value and change motor direction when pwm becomes either negative or positive
+ // 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;
motor1.write(m1_pwm);
@@ -286,108 +298,115 @@
motor2dir=1;
motor2.write(-m2_pwm);
}
-
- //hidsopce to check what the code does exactly
-
- scope.set(0,ref_angle1-angle1); //error1
- scope.set(1,ref_angle1);
- scope.set(2,m1_pwm);
- scope.set(3,ref_angle2-angle2);
- scope.set(4,ref_angle2);
- scope.set(5,servo_pwm*1000);
- scope.send();
-
}
-void servo_controller() // this function is used to stabalize the spatula with the servo
+void servo_controller() // Function for stabilizing the spatula with the servo
{
- if (theta < maxTheta ) { //servo can stabilize until a maximum theta
- servo_pwm=min_servo_pwm+(theta-minTheta)/diffTheta*res_servo;
+ // 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;
} else {
servo_pwm=max_servo_pwm;
}
- if (!servo_button) { // flip burger, spatula to max position
+
+ // The spatula goes to its maximum position to flip a burger if the button is pressed
+ if (!servo_button) {
servo_pwm=0.0014;
}
+ // Send the servo_pwm to the servo
servo.pulsewidth(servo_pwm);
}
-void my_emg()
+void my_emg() // This function prints the highest emg values to putty
{
- //This function is attached to a ticker so that the highest emg values are printed every second.
pc.printf("highest_emg1=%.4f\thighest_emg2=%.4f\thighest_emg3=%.4f\n\r", highest_emg1, highest_emg2, highest_emg3);
}
-void my_pos()
+void my_pos() // This function prints the reference position to putty
{
- //This function is attached to the same ticker as my_emg so that the reference position is printed every second instead of the highest emg values.
pc.printf("x_pos=%.4f\ty_pos=%.4f\tradius=%.4f\tangle=%.4f\n\r",ref_x,ref_y,radius,theta/pi*180.0);
}
int main()
{
pc.printf("RESET\n\r");
+
+ // Set the baud rate
pc.baud(115200);
- //Attach the Biquads to the Biquad chains
+ // Attach the Biquads to the Biquad chains
bqc11.add( &bq111 ).add( &bq112 ).add( &bq113 ).add( &bq121 ).add( &bq122 ).add( &bq123 );
- bqc13.add( &bq131);
+ bqc12.add( &bq131);
bqc21.add( &bq211 ).add( &bq212 ).add( &bq213 ).add( &bq221 ).add( &bq222 ).add( &bq223 );
- bqc23.add( &bq231);
+ bqc22.add( &bq231);
bqc31.add( &bq311 ).add( &bq312 ).add( &bq313 ).add( &bq321 ).add( &bq322 ).add( &bq323 );
- bqc33.add( &bq331);
+ bqc32.add( &bq331);
+
+ // Define the period of the pwm signals
+ motor1.period(0.02f);
+ motor2.period(0.02f);
- motor1.period(0.02f); // period of pwm signal for motor 1
- motor2.period(0.02f); // period of pwm signal for motor 2
- motor1dir=0; // setting direction to ccw
- motor2dir=0; // setting direction to ccw
+ // Set the direction of the motors to ccw
+ motor1dir=0;
+ motor2dir=0;
- pos_timer.attach(&my_emg, 1); // ticker used to print the maximum emg values every second
+ // Attaching the function my_emg() to the ticker print_timer
+ print_timer.attach(&my_emg, 1);
- //this while loop is used to determine what the highest possible value of the emg signals are and the threshold values are 1/5 of that.
- //this is done so that every user can use his own threshold value.
+ // While loop used for calibrating the emg thresholds, So that every user can use it
while (servo_button==1) {
- emg11=bqc13.step(fabs(bqc11.step(emg1.read()))); //filtered signal 1
- emg21=bqc23.step(fabs(bqc21.step(emg2.read()))); //filtered signal 2
- emg31=bqc33.step(fabs(bqc31.step(emg3.read()))); //filtered signal 3
- if(emg11>highest_emg1) {
- highest_emg1=emg11;
+ 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(emg21>highest_emg2) {
- highest_emg2=emg21;
+
+ if(emg2>highest_emg2) {
+ highest_emg2=emg2;
}
- if(emg31>highest_emg3) {
- highest_emg3=emg31;
+
+ if(emg3>highest_emg3) {
+ highest_emg3=emg3;
}
- threshold1=0.2*highest_emg1;
- threshold2=0.2*highest_emg2;
- threshold3=0.2*highest_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;
}
- //Attach the 'sample' function to the timer 'sample_timer'.
- //this ensures that 'sample' is executed every 0.002 seconds = 500 Hz
- sample_timer.attach(&activate_sample, samplefreq);
+ // Attach the function sample() to the ticker sample_timer
+ sample_timer.attach(&activate_sample, freq);
+
+ // Attach the function my_pos() to the ticker print_timer.
+ print_timer.attach(&my_pos, 1);
- //Attach the function my_pos to the timer pos_timer.
- //This ensures that the reference position is printed every second.
- pos_timer.attach(&my_pos, 1);
- control.attach(&activate_controller,m1_Ts); //Ticker for processing encoder input
- servo_control.attach(&activate_servo_control,servo_Ts);
+ // 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 (sample_go==true) {
- sample();
- sample_go = false; //change sample_go to false if sample() is finished
+ // Only take a sample when the go flag is true.
+ if (change_ref_go==true) {
+ change_ref();
+ change_ref_go = false;
}
- if(controller_go) { // go flag
+
+ // 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) {
servo_controller();
servo_go=false;