Rick Poppe / Mbed 2 deprecated final_control_v3

naam van een functie veranderd

Dependencies:   FastPWM HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@3:58378b78079d, 2016-11-01 (annotated)

Committer:
RiP
Date:
Tue Nov 01 15:00:40 2016 +0000
Revision:
3:58378b78079d
Parent:
2:afa5a01ad84b
Child:
4:b83460036800
final_control_v3; naam van een functie veranderd

Who changed what in which revision?

UserRevisionLine numberNew contents of line
mefix 0:3c99f1705565 1 #include "mbed.h"
mefix 0:3c99f1705565 2 #include "HIDScope.h"
mefix 0:3c99f1705565 3 #include "BiQuad.h"
mefix 0:3c99f1705565 4 #include "MODSERIAL.h"
mefix 0:3c99f1705565 5 #include "QEI.h"
mefix 0:3c99f1705565 6 #include "FastPWM.h"
mefix 0:3c99f1705565 7
mefix 0:3c99f1705565 8 // in gebruik: D(0(TX),1(RX),4(motor2dir),5(motor2pwm),6(motor1pwm),7(motor1dir),
mefix 0:3c99f1705565 9 //8(pushbutton),9(servoPWM),10(encoder),11(encoder),12(encoder),13(encoder)) A(0,1,2)(emg)
mefix 0:3c99f1705565 10
mefix 0:3c99f1705565 11 MODSERIAL pc(USBTX, USBRX);
mefix 0:3c99f1705565 12 HIDScope scope(6); // the amount of scopes to send to the pc
mefix 0:3c99f1705565 13
mefix 0:3c99f1705565 14 //Define objects
mefix 0:3c99f1705565 15
mefix 0:3c99f1705565 16 //Define the EMG inputs
mefix 0:3c99f1705565 17 AnalogIn emg1( A0 );
mefix 0:3c99f1705565 18 AnalogIn emg2( A1 );
mefix 0:3c99f1705565 19 AnalogIn emg3( A2 );
mefix 0:3c99f1705565 20
mefix 0:3c99f1705565 21 //Define motor outputs
mefix 0:3c99f1705565 22 DigitalOut motor1dir(D7); //direction of motor 1, attach at m1, set to 0: cw
mefix 0:3c99f1705565 23 FastPWM motor1(D6); // speed of motor 1
mefix 0:3c99f1705565 24 FastPWM motor2(D5); //speed of motor 2
mefix 0:3c99f1705565 25 DigitalOut motor2dir(D4); //direction of motor 2, attach at m2, set to 0: ccw
mefix 0:3c99f1705565 26 FastPWM servo(D9); //servo pwm
mefix 2:afa5a01ad84b 27 DigitalIn servo_button(PTC12); // servo flip
mefix 0:3c99f1705565 28
mefix 0:3c99f1705565 29 QEI Encoder1(D13,D12,NC,64,QEI::X4_ENCODING); //defining encoder
mefix 0:3c99f1705565 30 QEI Encoder2(D11,D10,NC,64,QEI::X4_ENCODING); //defining encoder
mefix 0:3c99f1705565 31
mefix 0:3c99f1705565 32 //Define the Tickers
mefix 0:3c99f1705565 33 Ticker pos_timer; // the timer which is used to print the position every second
mefix 0:3c99f1705565 34 Ticker sample_timer; // the timer which is used to decide when a sample needs to be taken
mefix 0:3c99f1705565 35 Ticker control; // Ticker for processing encoder input to motor output
mefix 0:3c99f1705565 36 Ticker servo_control; // Ticker for calling servo_control
mefix 0:3c99f1705565 37
mefix 0:3c99f1705565 38 //Initialize all variables
RiP 1:ba63033da653 39 volatile bool sample_go = false; // go flag sample()
RiP 1:ba63033da653 40 volatile bool controller_go=false; // go flag controller()
RiP 1:ba63033da653 41 volatile bool servo_go=false; // go flag servo_controller()
RiP 1:ba63033da653 42
mefix 0:3c99f1705565 43
RiP 1:ba63033da653 44 double highest_emg1; // the highest emg signal of emg input 1
RiP 1:ba63033da653 45 double highest_emg2; // the highest emg signal of emg input 2
RiP 1:ba63033da653 46 double highest_emg3; // the highest emg signal of emg input 3
RiP 1:ba63033da653 47 double threshold1; // the threshold which the first emg signal needs to surpass to do something
RiP 1:ba63033da653 48 double threshold2; // the threshold which the second emg signal needs to surpass to do something
RiP 1:ba63033da653 49 double threshold3; // the threshold which the third emg signal needs to surpass to do something
mefix 0:3c99f1705565 50 double samplefreq=0.002; // every 0.002 sec a sample will be taken this is a frequency of 500 Hz
RiP 1:ba63033da653 51 double emg11; // the first emg signal
RiP 1:ba63033da653 52 double emg21; // the second emg signal
RiP 1:ba63033da653 53 double emg31; // the third emg signal
mefix 0:3c99f1705565 54 double ref_x=0.0000; // the x reference position
mefix 0:3c99f1705565 55 double ref_y=0.0000; // the y reference position
mefix 0:3c99f1705565 56 double old_ref_x; // the old x reference
mefix 0:3c99f1705565 57 double old_ref_y; // the old y reference
mefix 0:3c99f1705565 58 double speed=0.00008; // the variable with which a speed is reached of 1cm/s
mefix 0:3c99f1705565 59 double theta=0.0; // angle of the arm
mefix 0:3c99f1705565 60 double radius=0.0; // radius of the arm
mefix 0:3c99f1705565 61
mefix 0:3c99f1705565 62 const double minRadius=0.43; // minimum radius of arm
mefix 0:3c99f1705565 63 const double maxRadius=0.62; // maximum radius of arm
mefix 0:3c99f1705565 64 const double min_y=-0.26; // minimum height which the spatula can reach
mefix 0:3c99f1705565 65 char key; // variable to place the keyboard input
mefix 0:3c99f1705565 66
mefix 0:3c99f1705565 67 double m1_pwm=0; //variable for PWM control motor 1
mefix 0:3c99f1705565 68 double m2_pwm=0; //variable for PWM control motor 2
mefix 0:3c99f1705565 69
mefix 0:3c99f1705565 70 const double m1_Kp = 35.16, m1_Ki = 108.8, m1_Kd = 2.84, m1_N = 100; // controller constants motor 1
mefix 0:3c99f1705565 71 double m1_v1 = 0, m1_v2 = 0; // Memory variables
mefix 0:3c99f1705565 72 const double m1_Ts = 0.01; // Controller sample time
mefix 0:3c99f1705565 73
mefix 2:afa5a01ad84b 74 const double m2_Kp = 36.24, m2_Ki = 108.41, m2_Kd = 3.03, m2_N = 100; // controller constants motor 2
mefix 0:3c99f1705565 75 double m2_v1 = 0, m2_v2 = 0; // Memory variables
mefix 0:3c99f1705565 76 const double m2_Ts = 0.01; // Controller sample time
mefix 0:3c99f1705565 77
mefix 0:3c99f1705565 78 const double pi=3.14159265359;
mefix 0:3c99f1705565 79 const double res = 64.0/(1.0/131.25*2.0*pi); // resolution on gearbox shaft per pulse
mefix 0:3c99f1705565 80 const double V_max=9.0; // maximum voltage supplied by trafo
mefix 0:3c99f1705565 81 const double pulleyRadius=0.0398/2.0; // pulley radius
mefix 0:3c99f1705565 82
mefix 0:3c99f1705565 83 double servo_pwm=0.0023; // duty cycle 1.5 ms 7.5%, min 0.5 ms 2.5%, max 2.5 ms 12.5%
mefix 2:afa5a01ad84b 84 const double minTheta=-38.0/180.0*pi; //minimum angle robot
mefix 2:afa5a01ad84b 85 const double maxTheta=-24.0/180.0*pi; // maximum angle to which the spatula can stabilise
mefix 0:3c99f1705565 86 const double diffTheta=maxTheta-minTheta; //difference between max and min angle of theta for stabilisation
RiP 3:58378b78079d 87 const double min_servo_pwm=0.0020; // corresponds to angle of theta -38 degrees
RiP 3:58378b78079d 88 const double max_servo_pwm=0.0022; // corresponds to angle of theta -24 degrees
mefix 0:3c99f1705565 89 const double res_servo=max_servo_pwm-min_servo_pwm; //resolution of servo pwm signal between min and max angle
mefix 0:3c99f1705565 90 const double servo_Ts=0.02;
mefix 0:3c99f1705565 91 bool z_push=false;
mefix 0:3c99f1705565 92
mefix 0:3c99f1705565 93 //Define the needed Biquad chains
mefix 0:3c99f1705565 94 BiQuadChain bqc11;
mefix 0:3c99f1705565 95 BiQuadChain bqc13;
mefix 0:3c99f1705565 96 BiQuadChain bqc21;
mefix 0:3c99f1705565 97 BiQuadChain bqc23;
mefix 0:3c99f1705565 98 BiQuadChain bqc31;
mefix 0:3c99f1705565 99 BiQuadChain bqc33;
mefix 0:3c99f1705565 100
mefix 0:3c99f1705565 101 //Define the BiQuads for the filter of the first emg signal
mefix 0:3c99f1705565 102 //Notch filter
mefix 0:3c99f1705565 103 BiQuad bq111(0.9795, -1.5849, 0.9795, 1.0000, -1.5849, 0.9589);
mefix 0:3c99f1705565 104 BiQuad bq112(0.9833, -1.5912, 0.9833, 1.0000, -1.5793, 0.9787);
mefix 0:3c99f1705565 105 BiQuad bq113(0.9957, -1.6111, 0.9957, 1.0000, -1.6224, 0.9798);
mefix 0:3c99f1705565 106 //High pass filter
mefix 0:3c99f1705565 107 //BiQuad bq121( 9.56543e-01, -1.91309e+00, 9.56543e-01, -1.91120e+00, 9.14976e-01 ); //Old biquad values
mefix 0:3c99f1705565 108 BiQuad bq121( 0.8956, -1.7911, 0.8956, 1.0000, -1.7814, 0.7941);
mefix 0:3c99f1705565 109 BiQuad bq122( 0.9192, -1.8385, 0.9192, 1.0000, -1.8319, 0.8450);
mefix 0:3c99f1705565 110 BiQuad bq123( 0.9649, -1.9298, 0.9649, 1.0000, -1.9266, 0.9403);
mefix 0:3c99f1705565 111 //Low pass filter
mefix 0:3c99f1705565 112 BiQuad bq131( 3.91302e-05, 7.82604e-05, 3.91302e-05, -1.98223e+00, 9.82385e-01 );
mefix 0:3c99f1705565 113
mefix 0:3c99f1705565 114 //Define the BiQuads for the filter of the second emg signal
mefix 0:3c99f1705565 115 //Notch filter
mefix 0:3c99f1705565 116 BiQuad bq211 = bq111;
mefix 0:3c99f1705565 117 BiQuad bq212 = bq112;
mefix 0:3c99f1705565 118 BiQuad bq213 = bq113;
mefix 0:3c99f1705565 119 //High pass filter
mefix 0:3c99f1705565 120 BiQuad bq221 = bq121;
mefix 0:3c99f1705565 121 BiQuad bq222 = bq122;
mefix 0:3c99f1705565 122 BiQuad bq223 = bq123;
mefix 0:3c99f1705565 123 //Low pass filter
mefix 0:3c99f1705565 124 BiQuad bq231 = bq131;
mefix 0:3c99f1705565 125
mefix 0:3c99f1705565 126 //Define the BiQuads for the filter of the third emg signal
mefix 0:3c99f1705565 127 //notch filter
mefix 0:3c99f1705565 128 BiQuad bq311 = bq111;
mefix 0:3c99f1705565 129 BiQuad bq312 = bq112;
mefix 0:3c99f1705565 130 BiQuad bq313 = bq113;
mefix 0:3c99f1705565 131 //High pass filter
mefix 0:3c99f1705565 132 BiQuad bq321 = bq121;
mefix 0:3c99f1705565 133 BiQuad bq323 = bq122;
mefix 0:3c99f1705565 134 BiQuad bq322 = bq123;
mefix 0:3c99f1705565 135 //low pass filter
mefix 0:3c99f1705565 136 BiQuad bq331 = bq131;
mefix 0:3c99f1705565 137
RiP 3:58378b78079d 138 void activate_sample()
mefix 0:3c99f1705565 139 {
RiP 1:ba63033da653 140 if (sample_go==true) {
mefix 0:3c99f1705565 141 // this if statement is used to see if the code takes too long before it is called again
RiP 3:58378b78079d 142 pc.printf("rate too high error in activate_sample\n\r");
mefix 0:3c99f1705565 143 }
mefix 0:3c99f1705565 144 //This sets the go flag for when the function sample needs to be called
RiP 1:ba63033da653 145 sample_go=true;
mefix 0:3c99f1705565 146 }
mefix 0:3c99f1705565 147
mefix 0:3c99f1705565 148 void activate_controller()
mefix 0:3c99f1705565 149 {
mefix 0:3c99f1705565 150 if (controller_go==true) {
mefix 0:3c99f1705565 151 // this if statement is used to see if the code takes too long before it is called again
mefix 0:3c99f1705565 152 pc.printf("rate too high error in activate_controller()\n\r");
mefix 0:3c99f1705565 153 }
mefix 0:3c99f1705565 154 controller_go=true; //activate go flag
mefix 0:3c99f1705565 155 }
mefix 0:3c99f1705565 156
mefix 0:3c99f1705565 157 void activate_servo_control()
mefix 0:3c99f1705565 158 {
mefix 0:3c99f1705565 159 if (servo_go==true) {
mefix 0:3c99f1705565 160 pc.printf("error servo");
mefix 0:3c99f1705565 161 }
mefix 0:3c99f1705565 162 servo_go=true; //activate go flag
mefix 0:3c99f1705565 163 }
mefix 0:3c99f1705565 164
mefix 0:3c99f1705565 165 void sample()
mefix 0:3c99f1705565 166 {
mefix 0:3c99f1705565 167 //This checks if a key is pressed and changes the variable key in the pressed key
mefix 0:3c99f1705565 168 if (pc.readable()==1) {
mefix 0:3c99f1705565 169 key=pc.getc();
mefix 0:3c99f1705565 170 }
mefix 0:3c99f1705565 171 //Read the emg signals and filter it
mefix 0:3c99f1705565 172
RiP 3:58378b78079d 173 //scope.set(0,emg1.read());
RiP 1:ba63033da653 174 emg11=bqc13.step(fabs(bqc11.step(emg1.read()))); //filtered signal 1
RiP 3:58378b78079d 175 //scope.set(1,emg11);
RiP 3:58378b78079d 176
RiP 3:58378b78079d 177 //scope.set(2,emg2.read());
RiP 1:ba63033da653 178 emg21=bqc23.step(fabs(bqc21.step(emg2.read()))); //filtered signal 2
RiP 3:58378b78079d 179 //scope.set(3,emg21);
RiP 3:58378b78079d 180
RiP 3:58378b78079d 181 //scope.set(4,emg3.read());
RiP 1:ba63033da653 182 emg31=bqc33.step(fabs(bqc31.step(emg3.read()))); //filtered signal 3
RiP 3:58378b78079d 183 //scope.set(5,emg31);
mefix 0:3c99f1705565 184
mefix 0:3c99f1705565 185 //remember what the reference was
mefix 0:3c99f1705565 186 old_ref_x=ref_x;
mefix 0:3c99f1705565 187 old_ref_y=ref_y;
mefix 0:3c99f1705565 188 //look if the emg signals go over the threshold and change the reference accordingly
RiP 1:ba63033da653 189 if (emg11>threshold1&&emg21>threshold2&&emg31>threshold3 || key=='d') {
mefix 0:3c99f1705565 190 ref_x=ref_x-speed;
mefix 0:3c99f1705565 191 ref_y=ref_y-speed;
mefix 0:3c99f1705565 192
RiP 1:ba63033da653 193 } else if (emg11>threshold1&&emg21>threshold2 || key == 'a' || key == 'z') {
mefix 0:3c99f1705565 194 ref_x=ref_x-speed;
mefix 0:3c99f1705565 195
RiP 1:ba63033da653 196 } else if (emg11>threshold1&&emg31>threshold3 || key == 's') {
mefix 0:3c99f1705565 197 ref_y=ref_y-speed;
mefix 0:3c99f1705565 198
RiP 1:ba63033da653 199 } else if (emg21>threshold2&&emg31>threshold3 || key == 'e' ) {
mefix 0:3c99f1705565 200 ref_x=ref_x+speed;
mefix 0:3c99f1705565 201 ref_y=ref_y+speed;
mefix 0:3c99f1705565 202
RiP 1:ba63033da653 203 } else if (emg21>threshold2 || key == 'q' ) {
mefix 0:3c99f1705565 204 ref_x=ref_x+speed;
mefix 0:3c99f1705565 205
RiP 1:ba63033da653 206 } else if (emg31>threshold3 || key == 'w') {
mefix 0:3c99f1705565 207 ref_y=ref_y+speed;
mefix 0:3c99f1705565 208 }
mefix 0:3c99f1705565 209
RiP 3:58378b78079d 210 if (key == 'z') {
RiP 3:58378b78079d 211 z_push=true;
RiP 3:58378b78079d 212 }
RiP 3:58378b78079d 213
mefix 0:3c99f1705565 214 if (key != 'z' && z_push) {
mefix 0:3c99f1705565 215 ref_x=0.0;
mefix 0:3c99f1705565 216 ref_y=0.0;
mefix 0:3c99f1705565 217 Encoder1.reset();
mefix 0:3c99f1705565 218 Encoder2.reset();
mefix 0:3c99f1705565 219 z_push=false;
mefix 0:3c99f1705565 220 }
mefix 0:3c99f1705565 221
mefix 0:3c99f1705565 222 // convert the x and y reference to the theta and radius reference
mefix 0:3c99f1705565 223 theta=atan(ref_y/(ref_x+minRadius));
mefix 0:3c99f1705565 224 radius=sqrt(pow(ref_x+minRadius,2)+pow(ref_y,2));
mefix 0:3c99f1705565 225
mefix 0:3c99f1705565 226 //look if the new reference is outside the possible range and revert back to the old reference if it is outside the range
mefix 0:3c99f1705565 227 if (radius < minRadius) {
mefix 0:3c99f1705565 228 if (key != 'z') {
mefix 0:3c99f1705565 229 ref_x=old_ref_x;
mefix 0:3c99f1705565 230 ref_y=old_ref_y;
mefix 0:3c99f1705565 231 }
mefix 0:3c99f1705565 232 } else if ( radius > maxRadius) {
mefix 0:3c99f1705565 233 ref_x=old_ref_x;
mefix 0:3c99f1705565 234 ref_y=old_ref_y;
mefix 0:3c99f1705565 235 } else if (ref_y<min_y) {
mefix 0:3c99f1705565 236 ref_y=old_ref_y;
mefix 0:3c99f1705565 237 }
mefix 0:3c99f1705565 238 theta=atan(ref_y/(ref_x+minRadius));
mefix 0:3c99f1705565 239 radius=sqrt(pow(ref_x+minRadius,2)+pow(ref_y,2));
RiP 3:58378b78079d 240
RiP 3:58378b78079d 241 //scope.send();
mefix 0:3c99f1705565 242 }
mefix 0:3c99f1705565 243
mefix 0:3c99f1705565 244 double PID( double err, const double Kp, const double Ki, const double Kd,
mefix 0:3c99f1705565 245 const double Ts, const double N, double &v1, double &v2 ) //discrete PIDF filter
mefix 0:3c99f1705565 246 {
mefix 0:3c99f1705565 247 const double a1 =-4/(N*Ts+2),
mefix 0:3c99f1705565 248 a2=-(N*Ts-2)/(N*Ts+2),
mefix 0:3c99f1705565 249 b0=(4*Kp + 4*Kd*N + 2*Ki*Ts+2*Kp*N*Ts+Ki*N*pow(Ts,2))/(2*N*Ts+4),
mefix 0:3c99f1705565 250 b1=(Ki*N*pow(Ts,2)-4*Kp-4*Kd*N)/(N*Ts+2),
mefix 0:3c99f1705565 251 b2=(4*Kp+4*Kd*N-2*Ki*Ts-2*Kp*N*Ts+Ki*N*pow(Ts,2))/(2*N*Ts+4);
mefix 0:3c99f1705565 252
mefix 0:3c99f1705565 253 double v=err-a1*v1-a2*v2;
mefix 0:3c99f1705565 254 double u=b0*v+b1*v1+b2*v2;
mefix 0:3c99f1705565 255 v2=v1;
mefix 0:3c99f1705565 256 v1=v;
mefix 0:3c99f1705565 257 return u;
mefix 0:3c99f1705565 258 }
mefix 0:3c99f1705565 259
mefix 0:3c99f1705565 260 void controller() //function for executing controller action
mefix 0:3c99f1705565 261 {
mefix 0:3c99f1705565 262
mefix 0:3c99f1705565 263 //converting radius and theta to gearbox angle
mefix 0:3c99f1705565 264 double ref_angle1=16*theta;
mefix 0:3c99f1705565 265 double ref_angle2=(-radius+minRadius)/pulleyRadius;
mefix 0:3c99f1705565 266
mefix 0:3c99f1705565 267 double angle1 = Encoder1.getPulses()/res; //get number of pulses (counterclockwise is positive)
mefix 0:3c99f1705565 268 double angle2 = Encoder2.getPulses()/res; //get number of pulses
mefix 0:3c99f1705565 269 m1_pwm = (PID(ref_angle1-angle1,m1_Kp,m1_Ki,m1_Kd,m1_Ts,m1_N,m1_v1,m1_v2))/V_max;
mefix 0:3c99f1705565 270 //divide by voltage to get pwm duty cycle percentage)
mefix 0:3c99f1705565 271 m2_pwm = (PID(ref_angle2-angle2,m2_Kp,m2_Ki,m2_Kd,m2_Ts,m2_N,m2_v1,m2_v2))/V_max;
mefix 0:3c99f1705565 272
mefix 0:3c99f1705565 273 //limit pwm value and change motor direction when pwm becomes either negative or positive
mefix 0:3c99f1705565 274 if (m1_pwm >=0.0f && m1_pwm <=1.0f) {
mefix 0:3c99f1705565 275 motor1dir=0;
mefix 0:3c99f1705565 276 motor1.write(m1_pwm);
mefix 0:3c99f1705565 277 } else if (m1_pwm < 0.0f && m1_pwm >= -1.0f) {
mefix 0:3c99f1705565 278 motor1dir=1;
mefix 0:3c99f1705565 279 motor1.write(-m1_pwm);
mefix 0:3c99f1705565 280 }
mefix 0:3c99f1705565 281
mefix 0:3c99f1705565 282 if (m2_pwm >=0.0f && m2_pwm <=1.0f) {
mefix 0:3c99f1705565 283 motor2dir=0;
mefix 0:3c99f1705565 284 motor2.write(m2_pwm);
mefix 0:3c99f1705565 285 } else if (m2_pwm < 0.0f && m2_pwm >= -1.0f) {
mefix 0:3c99f1705565 286 motor2dir=1;
mefix 0:3c99f1705565 287 motor2.write(-m2_pwm);
mefix 0:3c99f1705565 288 }
mefix 0:3c99f1705565 289
mefix 0:3c99f1705565 290 //hidsopce to check what the code does exactly
RiP 3:58378b78079d 291
mefix 2:afa5a01ad84b 292 scope.set(0,ref_angle1-angle1); //error1
mefix 0:3c99f1705565 293 scope.set(1,ref_angle1);
mefix 0:3c99f1705565 294 scope.set(2,m1_pwm);
mefix 0:3c99f1705565 295 scope.set(3,ref_angle2-angle2);
mefix 0:3c99f1705565 296 scope.set(4,ref_angle2);
RiP 3:58378b78079d 297 scope.set(5,servo_pwm*1000);
mefix 0:3c99f1705565 298 scope.send();
RiP 3:58378b78079d 299
mefix 0:3c99f1705565 300 }
mefix 0:3c99f1705565 301
mefix 2:afa5a01ad84b 302 void servo_controller() // this function is used to stabalize the spatula with the servo
mefix 0:3c99f1705565 303 {
mefix 2:afa5a01ad84b 304 if (theta < maxTheta ) { //servo can stabilize until a maximum theta
mefix 0:3c99f1705565 305 servo_pwm=min_servo_pwm+(theta-minTheta)/diffTheta*res_servo;
mefix 0:3c99f1705565 306 } else {
mefix 0:3c99f1705565 307 servo_pwm=max_servo_pwm;
mefix 0:3c99f1705565 308 }
RiP 3:58378b78079d 309 if (!servo_button) { // flip burger, spatula to max position
mefix 2:afa5a01ad84b 310 servo_pwm=0.0014;
RiP 3:58378b78079d 311 }
RiP 3:58378b78079d 312
mefix 0:3c99f1705565 313 servo.pulsewidth(servo_pwm);
mefix 0:3c99f1705565 314
mefix 0:3c99f1705565 315 }
mefix 0:3c99f1705565 316
RiP 1:ba63033da653 317 void my_emg()
RiP 1:ba63033da653 318 {
RiP 1:ba63033da653 319 //This function is attached to a ticker so that the highest emg values are printed every second.
RiP 1:ba63033da653 320 pc.printf("highest_emg1=%.4f\thighest_emg2=%.4f\thighest_emg3=%.4f\n\r", highest_emg1, highest_emg2, highest_emg3);
RiP 1:ba63033da653 321 }
RiP 1:ba63033da653 322
mefix 0:3c99f1705565 323
mefix 0:3c99f1705565 324 void my_pos()
mefix 0:3c99f1705565 325 {
RiP 1:ba63033da653 326 //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.
mefix 2:afa5a01ad84b 327 pc.printf("x_pos=%.4f\ty_pos=%.4f\tradius=%.4f\tangle=%.4f\n\r",ref_x,ref_y,radius,theta/pi*180.0);
mefix 0:3c99f1705565 328 }
mefix 0:3c99f1705565 329
mefix 0:3c99f1705565 330 int main()
mefix 0:3c99f1705565 331 {
mefix 0:3c99f1705565 332 pc.printf("RESET\n\r");
mefix 0:3c99f1705565 333 pc.baud(115200);
mefix 0:3c99f1705565 334
mefix 0:3c99f1705565 335 //Attach the Biquads to the Biquad chains
mefix 0:3c99f1705565 336 bqc11.add( &bq111 ).add( &bq112 ).add( &bq113 ).add( &bq121 ).add( &bq122 ).add( &bq123 );
mefix 0:3c99f1705565 337 bqc13.add( &bq131);
mefix 0:3c99f1705565 338 bqc21.add( &bq211 ).add( &bq212 ).add( &bq213 ).add( &bq221 ).add( &bq222 ).add( &bq223 );
mefix 0:3c99f1705565 339 bqc23.add( &bq231);
mefix 0:3c99f1705565 340 bqc31.add( &bq311 ).add( &bq312 ).add( &bq313 ).add( &bq321 ).add( &bq322 ).add( &bq323 );
mefix 0:3c99f1705565 341 bqc33.add( &bq331);
mefix 0:3c99f1705565 342
RiP 1:ba63033da653 343 motor1.period(0.02f); // period of pwm signal for motor 1
RiP 1:ba63033da653 344 motor2.period(0.02f); // period of pwm signal for motor 2
RiP 1:ba63033da653 345 motor1dir=0; // setting direction to ccw
RiP 1:ba63033da653 346 motor2dir=0; // setting direction to ccw
RiP 3:58378b78079d 347
RiP 1:ba63033da653 348 pos_timer.attach(&my_emg, 1); // ticker used to print the maximum emg values every second
RiP 3:58378b78079d 349
RiP 1:ba63033da653 350 //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.
RiP 1:ba63033da653 351 //this is done so that every user can use his own threshold value.
RiP 3:58378b78079d 352 while (servo_button==1) {
RiP 1:ba63033da653 353 emg11=bqc13.step(fabs(bqc11.step(emg1.read()))); //filtered signal 1
RiP 1:ba63033da653 354 emg21=bqc23.step(fabs(bqc21.step(emg2.read()))); //filtered signal 2
RiP 1:ba63033da653 355 emg31=bqc33.step(fabs(bqc31.step(emg3.read()))); //filtered signal 3
RiP 1:ba63033da653 356 if(emg11>highest_emg1) {
RiP 1:ba63033da653 357 highest_emg1=emg11;
RiP 1:ba63033da653 358 }
RiP 1:ba63033da653 359 if(emg21>highest_emg2) {
RiP 1:ba63033da653 360 highest_emg2=emg21;
RiP 1:ba63033da653 361 }
RiP 1:ba63033da653 362 if(emg31>highest_emg3) {
RiP 1:ba63033da653 363 highest_emg3=emg31;
RiP 1:ba63033da653 364 }
RiP 1:ba63033da653 365 threshold1=0.2*highest_emg1;
RiP 1:ba63033da653 366 threshold2=0.2*highest_emg2;
RiP 1:ba63033da653 367 threshold3=0.2*highest_emg3;
RiP 3:58378b78079d 368
RiP 1:ba63033da653 369 }
mefix 0:3c99f1705565 370
mefix 0:3c99f1705565 371 //Attach the 'sample' function to the timer 'sample_timer'.
mefix 0:3c99f1705565 372 //this ensures that 'sample' is executed every 0.002 seconds = 500 Hz
RiP 3:58378b78079d 373 sample_timer.attach(&activate_sample, samplefreq);
mefix 0:3c99f1705565 374
mefix 0:3c99f1705565 375 //Attach the function my_pos to the timer pos_timer.
RiP 1:ba63033da653 376 //This ensures that the reference position is printed every second.
RiP 3:58378b78079d 377 pos_timer.attach(&my_pos, 1);
mefix 0:3c99f1705565 378 control.attach(&activate_controller,m1_Ts); //Ticker for processing encoder input
mefix 0:3c99f1705565 379 servo_control.attach(&activate_servo_control,servo_Ts);
mefix 0:3c99f1705565 380
mefix 0:3c99f1705565 381 while(1) {
mefix 0:3c99f1705565 382 //Only take a sample when the go flag is true.
RiP 1:ba63033da653 383 if (sample_go==true) {
mefix 0:3c99f1705565 384 sample();
RiP 1:ba63033da653 385 sample_go = false; //change sample_go to false if sample() is finished
mefix 0:3c99f1705565 386 }
mefix 0:3c99f1705565 387 if(controller_go) { // go flag
mefix 0:3c99f1705565 388 controller();
mefix 0:3c99f1705565 389 controller_go=false;
mefix 0:3c99f1705565 390 }
mefix 0:3c99f1705565 391 if(servo_go) {
mefix 0:3c99f1705565 392 servo_controller();
mefix 0:3c99f1705565 393 servo_go=false;
mefix 0:3c99f1705565 394 }
mefix 0:3c99f1705565 395 }
mefix 0:3c99f1705565 396 }