naam van een functie veranderd
Dependencies: FastPWM HIDScope MODSERIAL QEI biquadFilter mbed
Diff: main.cpp
- Revision:
- 3:58378b78079d
- Parent:
- 2:afa5a01ad84b
- Child:
- 4:b83460036800
--- a/main.cpp Tue Nov 01 13:05:39 2016 +0000 +++ b/main.cpp Tue Nov 01 15:00:40 2016 +0000 @@ -18,9 +18,6 @@ AnalogIn emg2( A1 ); AnalogIn emg3( A2 ); -//Button used to calibrate the threshold values -DigitalIn button(PTC6); - //Define motor outputs DigitalOut motor1dir(D7); //direction of motor 1, attach at m1, set to 0: cw FastPWM motor1(D6); // speed of motor 1 @@ -87,8 +84,8 @@ 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.00217; // corresponds to angle of theta -32 degrees -const double max_servo_pwm=0.0023; // corresponds to angle of theta -43 degrees +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; @@ -138,11 +135,11 @@ //low pass filter BiQuad bq331 = bq131; -void sampleflag() +void activate_sample() { if (sample_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 sampleflag\n\r"); + 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; @@ -173,9 +170,17 @@ } //Read the emg signals and filter it + //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 old_ref_x=ref_x; @@ -202,6 +207,10 @@ ref_y=ref_y+speed; } + if (key == 'z') { + z_push=true; + } + if (key != 'z' && z_push) { ref_x=0.0; ref_y=0.0; @@ -219,8 +228,6 @@ if (key != 'z') { ref_x=old_ref_x; ref_y=old_ref_y; - } else if (key == 'z') { - z_push=true; } } else if ( radius > maxRadius) { ref_x=old_ref_x; @@ -230,6 +237,8 @@ } theta=atan(ref_y/(ref_x+minRadius)); radius=sqrt(pow(ref_x+minRadius,2)+pow(ref_y,2)); + + //scope.send(); } double PID( double err, const double Kp, const double Ki, const double Kd, @@ -279,13 +288,15 @@ } //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); + scope.set(5,servo_pwm*1000); scope.send(); + } void servo_controller() // this function is used to stabalize the spatula with the servo @@ -295,10 +306,10 @@ } else { servo_pwm=max_servo_pwm; } - if (!servo_button){ // flip burger, spatula to max position + if (!servo_button) { // flip burger, spatula to max position servo_pwm=0.0014; - } - + } + servo.pulsewidth(servo_pwm); } @@ -333,12 +344,12 @@ motor2.period(0.02f); // period of pwm signal for motor 2 motor1dir=0; // setting direction to ccw motor2dir=0; // setting direction to ccw - + pos_timer.attach(&my_emg, 1); // ticker used to print the maximum emg values every second - + //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 (button==1) { + 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 @@ -354,15 +365,16 @@ threshold1=0.2*highest_emg1; threshold2=0.2*highest_emg2; threshold3=0.2*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(&sampleflag, samplefreq); + sample_timer.attach(&activate_sample, samplefreq); //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); + 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);