Dependencies: FastPWM MODSERIAL QEI biquadFilter mbed
Fork of state_program by
Revision 7:1906d404cd1e, committed 2018-11-06
- Comitter:
- KingMufasa
- Date:
- Tue Nov 06 10:32:00 2018 +0000
- Parent:
- 6:e67250b8b100
- Commit message:
- newest version
Changed in this revision
main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- a/main.cpp Thu Nov 01 19:35:15 2018 +0000 +++ b/main.cpp Tue Nov 06 10:32:00 2018 +0000 @@ -5,15 +5,15 @@ #include <iostream> #include <string> -enum States {WaitState, MotorCalState, EMGCalState, HomingState, MovingState, GrippingState, ScrewingState, FailureState}; +enum States {WaitState, MotorCalState, EMGCalState, HomingState, DemoState ,MovingState, GrippingState, ScrewingState, FailureState}; States currentState = WaitState; DigitalIn startButton(D0); InterruptIn failureButton(D1); -DigitalIn grippingSwitch(SW2); -DigitalIn screwingSwitch(SW3); -DigitalIn gripDirection(D2); -DigitalIn screwDirection(D3); +DigitalIn gripperButton(D2); +DigitalIn directionSwitch(D3); +DigitalIn gripperMotorButton(D14); + Serial pc(USBTX, USBRX); @@ -23,8 +23,6 @@ QEI encoder1(D10, D11, NC, 8400, QEI::X4_ENCODING); QEI encoder2(D12, D13, NC, 8400, QEI::X4_ENCODING); //QEI encoder3(A4, A5), NC, 4200); -AnalogIn pot(A4); // Speed knob -AnalogIn pot2(A5); AnalogIn emg0(A0); AnalogIn emg1(A1); @@ -41,22 +39,22 @@ const double PI = 3.141592653589793238463; const double L1 = 0.328; const double L2 = 0.218; - double T1[3][3] { +double T1[3][3] { {0, -1, 0}, {1, 0, 0,}, {0, 0, 0,} }; - double T20[3][3] { +double T20[3][3] { {0, -1, 0}, {1, 0, -L1,}, {0, 0, 0,} }; - double H200[3][3] { +double H200[3][3] { {1, 0, L1+L2}, {0, 1, 0,}, {0, 0, 1,} }; - double Pe2 [3][1] { +double Pe2 [3][1] { {0}, {0}, {1} @@ -70,8 +68,13 @@ DigitalOut directionpin1(D4); // motor direction FastPWM pwmpin2 (D6); DigitalOut directionpin2 (D7); -double setPointX = 0.218; -double setPointY = 0.328; +FastPWM pwmpin3(A4); //motor pwm +DigitalOut directionpin3(D8); // motor direction +FastPWM pwmpin4(A5); +DigitalOut directionpin4(D9); + +double setPointX = 0.3; +double setPointY = 0.28; double qRef1; double qRef2; double qMeas1; @@ -86,19 +89,19 @@ double C[5][5]; -double Kp1 = 150; -double Ki1 = 1; -double Kd1 = 10; -double Kp2 = 50; -double Ki2 = 0.1; -double Kd2 = 10; +double Kp1 = 10; +double Ki1 = 0; +double Kd1 = 2; +double Kp2 = 10; +double Ki2 = 0; +double Kd2 = 2; const int samplepack = 250; //Amount of data points before one cycle completes volatile int counter = 0; //Counts the amount of readings this cycle volatile double total[4] = {0,0,0,0}; //Total difference between data points and the calibration value this cycle (x+, x-, y+, y-) double refvaluebic = 10; //Minimum total value for the motor to move (biceps) double refvaluecalf = 50; //Minimum total value for the motor to move (calfs) -double incr = 0.00005; //increment +double incr = 0.00002; //increment bool moving[4] = {0,0,0,0}; //x+, x-, y+, y- BiQuad hpf(0.8635,-1.7269,0.8635,-1.9359,0.9394); //5Hz High pass filter @@ -117,51 +120,57 @@ stateChanged = true; } + + + + + void measureEMG () { - total[0] += abs(bqc.step(emg0.read())); - total[1] += abs(bqc.step(emg1.read())); - total[2] += abs(bqc.step(emg2.read())); - total[3] += abs(bqc.step(emg3.read())); - counter++; - if (counter >= samplepack){ - moving[0] = 0; - moving[1] = 0; - moving[2] = 0; - moving[3] = 0; - if (total[0] >= refvaluebic){ - moving[0] = 1; + if (currentState == MovingState) { + total[0] += abs(bqc.step(emg0.read())); + total[1] += abs(bqc.step(emg1.read())); + total[2] += abs(bqc.step(emg2.read())); + total[3] += abs(bqc.step(emg3.read())); + counter++; + if (counter >= samplepack) { + moving[0] = 0; + moving[1] = 0; + moving[2] = 0; + moving[3] = 0; + if (total[0] >= refvaluebic) { + moving[0] = 1; + } + if (total[1] >= refvaluebic) { + moving[1] = 1; + } + if (total[2] >= refvaluecalf) { + moving[2] = 1; + } + if (total[3] >= refvaluecalf) { + moving[3] = 1; + } + pc.printf("Totals of {x+,x-,y+,y-} are %f, %f, %f, %f \r\n",total[0],total[1],total[2],total[3]); + pc.printf("Coordinates: (%f,%f)\r\n", setPointX, setPointY); + counter = 0; //Reset for next cycle + for (int i=0; i<4; i++) { //clear 'total' array + total[i] = 0; + } } - if (total[1] >= refvaluebic){ - moving[1] = 1; - } - if (total[2] >= refvaluecalf){ - moving[2] = 1; + + if(moving[0]) { + setPointX += incr; } - if (total[3] >= refvaluecalf){ - moving[3] = 1; + if (moving[1]) { + setPointX -= incr; } - //pc.printf("Totals of {x+,x-,y+,y-} are %f, %f, %f, %f \r\n",total[0],total[1],total[2],total[3]); - pc.printf("Coordinates: (%f,%f)\r\n", setPointX, setPointY); - counter = 0; //Reset for next cycle - for (int i=0;i<4;i++){ //clear 'total' array - total[i] = 0; + if (moving[2]) { + setPointY += incr; + } + if (moving[3]) { + setPointY -= incr; } } - - if(moving[0]){ - setPointX += incr; - } - if (moving[1]){ - setPointX -= incr; - } - if (moving[2]){ - setPointY += incr; - } - if (moving[3]){ - setPointY -= incr; - } - } double measureVelocity (int motor) @@ -198,19 +207,19 @@ void measurePosition() // Measure actual angle position with the encoder { - pulses1 = encoder2.getPulses(); // motor 1 is on M2 and ENC2 of biorobotics shield - pulses2 = encoder1.getPulses(); // motor 2 is on M1 and ENC1 of biorobotics shield + pulses1 = encoder2.getPulses(); // motor 1 is on M2 and ENC2 of biorobotics shield + pulses2 = encoder1.getPulses(); // motor 2 is on M1 and ENC1 of biorobotics shield qMeas1 = (pulses1) * 2 * PI / 8400 +140.7*PI/180 ; // Calculate the angle relative to the starting point (8400 pulses per revolution) + offset - qMeas2 = (pulses2) * 2 * PI / 8400 - 93*PI/180 ; + qMeas2 = (pulses2) * 2 * PI / 8400 -87*PI/180; } void getMotorControlSignal () // Milestone 1 code, not relevant anymore { - double potSignal = pot.read() * 2 - 1; // read pot and scale to motor control signal + //double potSignal = pot.read() * 2 - 1; // read pot and scale to motor control signal //pc.printf("motor control signal = %f\n", posampleTimeignal); - u1 = potSignal; - u2 = potSignal; + u1 = 0; + u2 = 0; } template<std::size_t N, std::size_t M, std::size_t P> @@ -239,8 +248,8 @@ void inverseKinematics () { - if (currentState == MovingState) { // Only in the HomingState should the qRef1, qRef2 consistently change - + if (currentState == MovingState || currentState == DemoState) { // Only in the HomingState should the qRef1, qRef2 consistently change + double Pe_set[3][1] { // defining setpoint location of end effector {setPointX}, //setting xcoord to pot 1 {setPointY}, // setting ycoord to pot 2 @@ -306,7 +315,7 @@ //Determing 'Pulling" force to setpoint - double kspring= 1; //virtual stifness of the force + double kspring= 0.1; //virtual stifness of the force double Fs[3][1] { //force vector from end effector to setpoint {kspring*Pe_set[0][0] - kspring*Pe0[0][0]}, {kspring*Pe_set[1][0] - kspring*Pe0[1][0]}, @@ -331,7 +340,7 @@ //Calculating joint behaviour - double b1 =10; + double b1 =1; double b2 =1; //joint friction coefficent //double sampleTime = 1/1000; //Time step to reach the new angle @@ -356,9 +365,9 @@ void PID_controller() // Put the error trough PID control to make output 'u' { - if (currentState >= HomingState && currentState < FailureState){ - // Should only work when we move the robot to a defined position - error1 = qRef1 - qMeas1; + if (currentState >= HomingState && currentState < FailureState) { + // Should only work when we move the robot to a defined position + error1 = qRef1 - qMeas1; error2 = qRef2 - qMeas2; static double errorIntegral1 = 0; @@ -369,7 +378,6 @@ //Ki = pot2.read() * 0.5; //Only Kd is controlled by a pot, Kp and Ki are constant // Proportional part: - double Kp = pot2.read() * 0.001; double u_k1 = Kp1 * error1; double u_k2 = Kp2 * error2; @@ -392,8 +400,8 @@ // Sum all parsampleTime u1 = u_k1 + u_i1 + u_d1; u2 = u_k2 + u_i2 + u_d2; - - + + } } @@ -403,6 +411,10 @@ pwmpin1 = fabs(u1); directionpin2 = u2 > 0.0f; // Either true or false pwmpin2 = fabs(u2); + directionpin3 = u3 > 0.0f; // Either true or false + pwmpin3 = fabs(u3); + directionpin4 = u4 > 0.0f; // Either true or false + pwmpin4 = fabs(u4); } void stateMachine () @@ -444,7 +456,7 @@ // Add stuff you do every loop getMotorControlSignal(); - if (stateTimer >= 3.0f && fabs(measureVelocity(1)) < 100 && screwingSwitch.read() == false) { //TODO: add && fabs(measureVelocity(2)) < 0.1f + if (stateTimer >= 3.0f && fabs(measureVelocity(1)) < 100 && startButton.read() == false && fabs(measureVelocity(2)) < 0.1f) { //TODO: add //TODO: Add reset of encoder2 led2 = 1; encoder1.reset(); // Reset encoder for the 0 position @@ -478,13 +490,18 @@ led1 = 0; led2 = 0; // EmisampleTime yellow together //TODO: Set qRef1 and qRef2 - qRef1 = 90 * PI / 180; - qRef2 = -qRef1 + 0 *PI/180; + qRef1 = 60 * PI / 180; + qRef2 = -90 *PI/180; stateChanged = false; } // Nothing extra happens till robot reaches starting position and button is pressed - + if (gripperButton.read() == false) { //TODO: Also add position condition + led1 = 1; + led2 = 1; + currentState = DemoState; + stateChanged = true; + } if (startButton.read() == false) { //TODO: Also add position condition led1 = 1; led2 = 1; @@ -492,6 +509,31 @@ stateChanged = true; } break; + case DemoState: + if (stateChanged == true) { + stateTimer.reset(); + setPointX = 0.15; + setPointY = 0.3; + stateTimer.start(); + stateChanged = false; + } + static double blinkTimer = 0; + if (blinkTimer >= 0.5) { + led2 = !led2; + blinkTimer = 0; + } + blinkTimer += sampleTime; + if (stateTimer >= 5.0) { + setPointX = -0.1; + setPointY = 0.3; + } + + if (stateTimer >= 20.0) { + stateTimer.reset(); + currentState = HomingState; + stateChanged = true; + } + break; case MovingState: if (stateChanged == true) { // Entry action: all the things you do once in this state; @@ -501,7 +543,7 @@ stateChanged = false; } - if (grippingSwitch.read() == false) { + if (gripperButton.read() == false) { led1 = 1; led2 = 1; led3 = 1; @@ -515,24 +557,35 @@ // Entry action: all the things you do once in this state; led2 = 0; led3 = 0; // EmisampleTime light blue together + stateTimer.reset(); + stateTimer.start(); stateChanged = false; } - if (gripDirection == true) { - // Close gripper - } else { - // Open gripper + if (gripperMotorButton.read() == false) { + pc.printf("Button pressed \r\n"); + u3 = 0.4; + if (directionSwitch == true) { + // Close gripper, so positive direction + } else { + // Open gripper + u3 = u3 * -1; + } + } else { // If the button isn't pressed, turn off motor + u3 = 0; } - if (screwingSwitch.read() == false) { + if (gripperButton.read() == false && stateTimer >= 2.0f) { led2 = 1; led3 = 1; + u3 = 0; currentState = ScrewingState; stateChanged = true; } if (startButton.read() == false) { led2 = 1; led3 = 1; + u3 = 0; currentState = MovingState; stateChanged = true; } @@ -545,15 +598,26 @@ stateChanged = false; } - if (screwDirection == true) { - // Screw + if (gripperMotorButton.read() == false) { + u4 = 0.3; + u3 = -0.35; + if (directionSwitch == true) { + // Screw + } else { + // Unscrew + u4 = u4 * -1; + u3 = u3 * -1; + } } else { - // Unscrew + u4 = 0; + u3 = 0; } if (startButton.read() == false) { led1 = 1; led3 = 1; + u3 = 0; + u4 = 0; currentState = MovingState; stateChanged = true; } @@ -568,12 +632,12 @@ stateChanged = false; } - static double blinkTimer = 0; - if (blinkTimer >= 0.5) { + static double blinkTimer2 = 0; + if (blinkTimer2 >= 0.5) { led1 = !led1; - blinkTimer = 0; + blinkTimer2 = 0; } - blinkTimer += sampleTime; + blinkTimer2 += sampleTime; break; } @@ -588,7 +652,7 @@ void mainLoop () { - // Add measure, motor controller and output function + // Add measure, motor controller and output function measureAll(); stateMachine(); PID_controller(); @@ -597,8 +661,15 @@ int main() { + startButton.mode(PullUp); + failureButton.mode(PullUp); + gripperButton.mode(PullUp); + directionSwitch.mode(PullUp); + gripperMotorButton.mode(PullUp); pc.baud(115200); pc.printf("checkpoint 1\n"); + //pwmpin1.period(sampleTime); + //pwmpin2.period(sampleTime); bqc.add(&hpf).add(¬ch).add(&lpf); //Add bqfilters to bqc mainTicker.attach(mainLoop, sampleTime); failureButton.fall(&switchToFailureState); // When button is pressed FailureState is activated @@ -607,7 +678,7 @@ //pc.printf("State = %i\n", currentState); //int pulses = encoder1.getPulses(); //pc.printf("pulses = %i\n", pulses); - // pc.printf("v = %f\n", v); + // pc.printf("v = %f\n", v); pc.printf("Error1 = %f Error2 = %f \n qRef1 = %f rQref2 = %f \r\n qMeas1 = %f qMeas2 = %f \n, Pulses1 = %f Pulses2 = %f \n", error1,error2,qRef1,qRef2,qMeas1,qMeas2, pulses1, pulses2); wait(2); }