State machine
Dependencies: HIDScope QEI biquadFilter mbed
Fork of State_machine by
Revision 7:af586102d80c, committed 2018-11-07
- Comitter:
- CasperK
- Date:
- Wed Nov 07 13:51:55 2018 +0000
- Parent:
- 6:344e075c1221
- Commit message:
- Final code
Changed in this revision
Constants.h | Show annotated file Show diff for this revision Revisions of this file |
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 344e075c1221 -r af586102d80c Constants.h --- a/Constants.h Fri Nov 02 09:16:57 2018 +0000 +++ b/Constants.h Wed Nov 07 13:51:55 2018 +0000 @@ -7,8 +7,8 @@ AnalogIn potmeter2(A4); DigitalIn button1(D2); DigitalIn button2(D3); -DigitalOut directionpin1(D4); -DigitalOut directionpin2(D7); +DigitalOut directionpin2(D4); +DigitalOut directionpin1(D7); QEI motor1(D13,D12,NC, 32); QEI motor2(D11,D10,NC, 32); @@ -41,6 +41,7 @@ Ticker sample_timer; Ticker MotorsTicker; +Ticker stateticker; Timer timer; //for testing purposes @@ -48,7 +49,7 @@ DigitalOut ledgreen(LED_GREEN); DigitalOut ledblue(LED_BLUE); //MODSERIAL pc(USBTX, USBRX); -HIDScope scope(6); +//HIDScope scope(6); bool emg0Bool = 0; // I don't know if these NEED to be global, but when I tried to put them in they wouldn't work... int emg0Ignore = 0; @@ -73,4 +74,48 @@ volatile float pwm_value1 = 0.0; volatile float pwm_value2 = 0.0; +const float C1 = 3.0; //motor 1 gear ratio +const float C2 = 0.013; //motor 2 gear ratio/radius of the circular gear in m +const float length = 0.300; //length in m (placeholder) +const float Timestep = 0.01; + +volatile bool x_direction = true; + +volatile float x_position = length; +volatile float y_position = 0.0; +volatile float old_x_position; +volatile float old_y_position; +volatile float x_correction; +volatile float old_x_correction; +volatile float old_motor1_angle; +volatile float old_motor2_angle; +volatile float motor1_angle = 0.0; //sawtooth gear motor +volatile float motor2_angle = 0.0; //rotational gear motor +volatile float direction; + +//values of PID controller +const float Kp = 5; +const float Ki = 0.02; +const float Kd = 0.05; +volatile float Output1 = 0 ; //Starting value +volatile float Output2 = 0 ; //Starting value +volatile float P1 = 0; //encoder value +volatile float P2 = 0; +volatile float e1 = 0 ; //Starting value +volatile float e2 = 0 ; //Starting value +volatile float e3 = 0; +volatile float f1 = 0 ; //Starting value +volatile float f2 = 0 ; //Starting value +volatile float f3 = 0; +volatile float df3 = 0; +volatile float if3 = 0; +volatile float de3 = 0; +volatile float ie3 = 0; + +volatile float Output_Last1; // Remember previous position +volatile float Output_Last2; // Remember previous position +volatile float Y1 = 0; // Value that is outputted to motor control +volatile float Y2 = 0; // Value that is outputted to motor control +volatile float P_Last = 0; // Starting position + #endif \ No newline at end of file
diff -r 344e075c1221 -r af586102d80c main.cpp --- a/main.cpp Fri Nov 02 09:16:57 2018 +0000 +++ b/main.cpp Wed Nov 07 13:51:55 2018 +0000 @@ -1,28 +1,166 @@ #include "mbed.h" #include <stdio.h> #include "QEI.h" -#include "HIDScope.h" +//#include "HIDScope.h" //#include "MODSERIAL.h" #include "BiQuad.h" #include "math.h" #include "Constants.h" + #define IGNORECOUNT 100 +#define PI 3.141592f //65358979323846 // pi + +void yDirection() { + //direction of the motion + if (emg0Bool && !emg1Bool) { //if a is pressed and not d, move to the left + direction = -1; + } + else if (!emg0Bool && emg1Bool) { //if d is pressed and not a, move to the right + direction = 1; + } + + if (emg0Bool || emg1Bool){ + //correction from motor 1 to keep x position the same + + + //calculating the motion + old_y_position = y_position; + y_position = old_y_position + (0.0003f * direction); + + + old_motor1_angle = motor1_angle; + motor1_angle = asin( y_position / length )* C1; //saw tooth motor angle in rad + if (y_position > 0.29f){ + y_position = 0.29f; + } + else if(y_position <0){ + y_position = 0; + } + //correction on x- axis + old_x_correction = x_correction; + x_correction = old_x_correction - (length * cos(old_motor1_angle / C1)- length * cos(motor1_angle/ C1)); // old x + correction + if (x_position > 0.35f- x_correction ){ + x_position = 0.35f- x_correction; + } + else if (x_position-x_correction < 0.0f){ + x_position = 0.0f; + } + + motor2_angle = ( x_position + x_correction - (length * cos(motor1_angle /C1 ))) / C2; + //reset the boolean, for demo purposes + emg2Bool = false; + } +} + +void xDirection () { + //if the button is pressed, reverse the y direction + if (!button2) { + x_direction = !x_direction; + wait(0.5f); + } + + if (emg2Bool) { //if w is pressed, move up/down + //direction of the motion + if (x_direction) { + direction = 1.0f; + } + else if (!x_direction) { + direction = -1.0f; + } + + //calculating the motion + old_x_position = x_position; + x_position = old_x_position + (0.0003f * direction); + if (x_position > 0.35f- x_correction ){ + x_position = 0.35f- x_correction; + } + else if (x_position-x_correction < 0.0f){ + x_position = 0.0f; + } + + motor2_angle =( x_position + x_correction - (length * cos( motor1_angle / C1 ))) /C2 ; // sawtooth-gear motor angle in rad + + //reset the boolean, for demo purposes + emg2Bool = false; + } +} + +void PIDController1() { + P1 = motor1.getPulses() / 8400.0f * 2.0f * PI; //actual motor angle in rad + e1 = e2; + e2 = e3; + e3 = motor1_angle - P1; + de3 = (e3-e2)/Timestep; + ie3 = ie3 + e3*Timestep; + Output1 = Kp * e3 + Ki * ie3 + Kd * de3; + + Y1 = 0.5f * Output1; + + if (Y1 >= 1){ + Y1 = 1; + } + else if (Y1 <= -1){ + Y1 = -1; + } +} + +void PIDController2() { + P2 = motor2.getPulses() / 8400.0f * 2.0f*PI; // actual motor angle in rad + f2 = f3; + f3 = motor2_angle - P2; + df3 = (f3-f2)/Timestep; + if3 = if3 + f3*Timestep; + Output2 = Kp * f3 + Ki * if3 + Kd * df3; + Y2 = 0.5f * Output2; + + if (Y2 >= 1){ + Y2 = 1; + } + else if (Y2 <= -1){ + Y2 = -1; + } +} + +void ControlMotor1() { + if (Y1 > 0.0f) { + Y1 = 0.6f * Y1 + 0.4f; + directionpin1 = false; + } + else if(Y1 < -0.0f){ + Y1 = -0.4f + 0.6f * Y1; + directionpin1 = true; + } + + pwm_value1 = fabs(Y1); +} + +void ControlMotor2() { + if (Y2 > 0.0f) { + Y2 = 0.6f * Y2 + 0.4f; + directionpin2 = true; + } + else if(Y2 < -0.0f){ + Y2 = -0.4f + 0.6f * Y2; + directionpin2 = false; + } + + pwm_value2 = fabs(Y2); +} /* * Calibration functions */ - void positionCalibration() { while(!button1) { - directionpin1 = true; - pwm_value1 = 0.7f; + directionpin2 = true; + pwmpin1 = 0.5f; } - pwm_value1 = 0.0f; while(!button2) { - directionpin2 = true; - pwm_value2 = 0.6f; + directionpin1 = true; + pwmpin2 = 0.4f; } - pwm_value2 = 0.0f; + pwmpin1 = 0.0f; + pwmpin2 = 0.0f; if(!next_switch) { CurrentState = Movement; @@ -56,7 +194,7 @@ } void CalibrateEMG1(){ // calibrates only one muscle activities as testing concluded that it was unnessecary to do this for all muscles. - ledgreen = !ledgreen; + ledred = !ledred; int C = 500; // half a second at 1000Hz double A0=0, A1=0, A2=0, A3=0, A4=0; double emg1FiAbs; @@ -76,11 +214,11 @@ wait(0.001f); } Calibration1 = (A0+A1+A2+A3+A4)/5*0.4; // average of the 5 highest values x 0,4 to create the threshold - ledgreen = !ledgreen; + ledred = !ledred; } void CalibrateEMG2(){ // calibrates only one muscle activities as testing concluded that it was unnessecary to do this for all muscles. - ledblue = !ledblue; + ledred = !ledred; int C = 500; // half a second at 1000Hz double A0=0, A1=0, A2=0, A3=0, A4=0; double emg2FiAbs; @@ -100,7 +238,7 @@ wait(0.001f); } Calibration2 = (A0+A1+A2+A3+A4)/5*0.4; // average of the 5 highest values x 0,4 to create the threshold - ledblue = !ledblue; + ledred = !ledred; } /* @@ -207,7 +345,12 @@ * function that runs the Kinematics and PID controller */ void movement() { - + xDirection(); //call the function to move in the y direction + yDirection(); //call the function to move in the x direction + PIDController1(); + PIDController2(); + ControlMotor1(); + ControlMotor2(); } void move_motors() { @@ -227,6 +370,7 @@ break; case PositionCalibration: + ledred = true; ledgreen = true; ledblue = false; //turn on the blue light positionCalibration(); @@ -242,20 +386,22 @@ } void KillInterrupt() { - //turn off the motors - pwm_value1 = 0; - pwm_value2 = 0; - //detach all the tickers MotorsTicker.detach(); sample_timer.detach(); + stateticker.detach(); + + //turn off the motors + pwm_value1 = 0.0f; + pwm_value2 = 0.0f; + pwmpin1 = 0.0f; + pwmpin2 = 0.0f; //burn the red light ledred = false; ledgreen = true; ledblue = true; - wait(2.0f); //give the person time to release the button bool b = true; //have the program get stuck in a while loop @@ -268,7 +414,9 @@ b = false; } } - StateFunction(); //get back to the state function + wait(1.5f); + MotorsTicker.attach(&move_motors, Timestep); + stateticker.attach(StateFunction, Timestep); } int main() @@ -283,11 +431,11 @@ ledblue = true; kill_switch.fall(KillInterrupt); //attach the kill switch to the KillInterrupt function - MotorsTicker.attach(&move_motors, 0.02f); //ticker at 50Hz + MotorsTicker.attach(&move_motors, Timestep); //ticker at 50Hz + stateticker.attach(StateFunction, Timestep); CurrentState = EmgCalibration; //start at the calibration state - while (true) { - StateFunction(); //keep running the state machine + while (true) { } } \ No newline at end of file