for De Jesse
Dependencies: FastPWM MODSERIAL QEI biquadFilter mbed
Fork of state_program1 by
main.cpp
- Committer:
- KingMufasa
- Date:
- 2018-11-06
- Revision:
- 9:6ce7218be0ee
- Parent:
- 8:5896424eb539
File content as of revision 9:6ce7218be0ee:
#include "mbed.h" #include "FastPWM.h" #include "QEI.h" #include "BiQuad.h" #include <iostream> #include <string> enum States {WaitState, MotorCalState, EMGCalState, HomingState, MovingState, GrippingState, ScrewingState, FailureState}; States currentState = WaitState; DigitalIn startButton(D0); InterruptIn failureButton(D1); DigitalIn gripperButton(D2); DigitalIn directionSwitch(D3); DigitalIn gripperMotorButton(D14); Serial pc(USBTX, USBRX); DigitalOut led1(LED1); // Red led DigitalOut led2(LED2); // Green led DigitalOut led3(LED3); // Blue led QEI encoder1(D10, D11, NC, 8400, QEI::X4_ENCODING); QEI encoder2(D12, D13, NC, 8400, QEI::X4_ENCODING); AnalogIn emg0(A0); AnalogIn emg1(A1); AnalogIn emg2(A2); AnalogIn emg3(A3); bool stateChanged = true; Ticker mainTicker; Timer stateTimer; Ticker calTimer; //Ticker that waits (to prepare the person) Ticker EMGsampler; //Ticker that samples the EMG const double sampleTime = 0.001; const double PI = 3.141592653589793238463; const double L1 = 0.328; // Length of Arm 1 const double L2 = 0.218; // Length of Arm 2 double T1[3][3] { // Twist of joints in reference configuration {0, -1, 0}, {1, 0, 0,}, {0, 0, 0,} }; double T20[3][3] { {0, -1, 0}, {1, 0, -L1,}, {0, 0, 0,} }; double H200[3][3] { {1, 0, L1+L2}, {0, 1, 0,}, {0, 0, 1,} }; double Pe2 [3][1] { {0}, {0}, {1} }; double u1; // Motor output of the long link double u2; // Motor of the short link double u3; // Motor of the gripper double u4; // Motor of the screwer FastPWM pwmpin1(D5); // Motor pwm DigitalOut directionpin1(D4); // Motor direction FastPWM pwmpin2 (D6); DigitalOut directionpin2 (D7); FastPWM pwmpin3(A4); DigitalOut directionpin3(D8); FastPWM pwmpin4(A5); DigitalOut directionpin4(D9); double setPointX = 0.3; // Declaring intial setpoint that robot moves to in the moving state where EMG can then be used double setPointY = 0.28; double qRef1; double qRef2; double qMeas1; double qMeas2; double C[5][5]; double Kp1 = 10; // PID control values of motors 1 and 2 that actuate two different arms 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.00002; // Increment bool moving[4] = {0,0,0,0}; //x+, x-, y+, y- volatile int waitingcounter = 0; //How many iterations of waiting have been done int countermax = 20; //counter until when should be waited volatile int processnow = 0; //Point in the calibration process volatile double armresttot = 0; volatile double armflextot = 0; volatile double legresttot = 0; volatile double legflextot = 0; const int samplecal = 2500; //Amount of data points to consider in the calibration double ratio = samplecal/samplepack; volatile double tot = 0; //Total measured EMG of the current type. volatile double armcal; //Calibated value for arm measurements volatile double legcal; //Calibrated value for leg measurements BiQuad hpf(0.8635,-1.7269,0.8635,-1.9359,0.9394); //5 Hz High pass filter BiQuad notch(0.9922,-1.6054,0.9922,-1.6054,0.9844); //50 Hz Notch filter BiQuad lpf(0.9780,1.9561,0.9780,1.9556,0.9565); //250Hz Low pass filter BiQuadChain bqc; void switchToFailureState () { failureButton.fall(NULL); // Detaches button, so it can only be activated once led1 = 0; led2 = 1; led3 = 1; pc.printf("SYSTEM FAILED\n"); currentState = FailureState; stateChanged = true; } void measureEMG () { 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(moving[0]) { setPointX += incr; } if (moving[1]) { setPointX -= incr; } if (moving[2]) { setPointY += incr; } if (moving[3]) { setPointY -= incr; } } } double measureVelocity (int motor) { static double lastPulses = 0; double currentPulses; static double velocity = 0; static int i = 0; if (i == 10) { // Encoder is not accurate enough, so with 1000 Hz the velocity can only be 0, 1000, 2000 or 3000 pulses/s switch (motor) { // Check which motor to measure case 1: currentPulses = encoder1.getPulses(); break; case 2: currentPulses = encoder2.getPulses(); break; } double deltaPulses = currentPulses - lastPulses; velocity = deltaPulses / (sampleTime * 10); // Velocity in pulses/s lastPulses = currentPulses; i = 0; } else { i += 1; } return velocity; } void measurePosition() // Measure actual angle position with the encoder { double pulses1 = encoder2.getPulses(); // motor 1 is on M2 and ENC2 of biorobotics shield double pulses2 = encoder1.getPulses(); // motor 2 is on M1 and ENC1 of biorobotics shield // Calculate the angle relative to the '0' point + offset (we have 8400 pulses per revolution) qMeas1 = (pulses1) * 2 * PI / 8400 +140.7 * PI / 180; qMeas2 = (pulses2) * 2 * PI / 8400 -87 * PI / 180; } template<std::size_t N, std::size_t M, std::size_t P> void mult(double A[N][M], double B[M][P]) // Matrix multiplication function calls matrix sizes and matrixs A and B { for( int n =0; n < 5; n++) { for(int p =0; p < 5; p++) { C[n][p] =0; } } for (int n = 0; n < N; n++) { for (int p = 0; p < P; p++) { double num = 0; for (int m = 0; m < M; m++) { num += A[n][m] * B[m][p]; } C[n][p] = num; } } } void inverseKinematics () { if (currentState == MovingState) { // Only in the MovingState should the qRef1, qRef2 change every sampleTime double Pe_set[3][1] { // Defining setpoint location of end effector {setPointX}, // Setting xcoord to setPointX {setPointY}, // Setting ycoord to setPointY {1} }; // Calculating new H matrix double T1e[3][3] { {cos(qRef1), -sin(qRef1), 0}, {sin(qRef1), cos(qRef1), 0}, {0, 0, 1} }; double T20e[3][3] { {cos(qRef2), -sin(qRef2), L1-L1*cos(qRef2)}, {sin(qRef2), cos(qRef2), -L1*sin(qRef2)}, {0, 0, 1} }; mult<3,3,3>(T1e,T20e); // Matrix multiplication double H201[3][3] { {C[0][0],C[0][1],C[0][2]}, {C[1][0],C[1][1],C[1][2]}, {C[2][0],C[2][1],C[2][2]} }; mult<3,3,3>(H201,H200); // Matrix multiplication double H20 [3][3] { {C[0][0],C[0][1],C[0][2]}, {C[1][0],C[1][1],C[1][2]}, {C[2][0],C[2][1],C[2][2]} }; mult<3,3,1>(H20,Pe2); // Matrix multiplication double Pe0[3][1] { {C[0][0]}, {C[1][0]}, {C[2][0]} }; double pe0x = Pe0[0][0]; // Seperating coordinates of end effector location double pe0y = Pe0[1][0]; // Determing the jacobian double T_1[3][1] { {1}, {T1[0][2]}, {T1[1][2]} }; double T_2[3][1] { {1}, {L1*sin(qRef1)}, {-L1*cos(qRef1)} }; double J[3][2] { {T_1[0][0], T_2[0][0]}, {T_1[1][0], T_2[1][0]}, {T_1[2][0], T_2[2][0]} }; // Determing 'Pulling" force to setpoint 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]}, {kspring*Pe_set[2][0] - kspring*Pe0[2][0]} }; double Fx = kspring*setPointX - kspring*pe0x; double Fy = kspring*setPointY - kspring*pe0y; double W0t[3][1] { {pe0x*Fy - pe0y*Fx}, {Fx}, {Fy} }; double Jt[2][3] { // Transposing jacobian matrix {J[0][0], J[1][0], J[2][0]}, {T_2[0][0], T_2[1][0], T_2[2][0]} }; mult<2,3,1>(Jt,W0t); double tau_st1 = C[0][0]; double tau_st2 = C[1][0]; // Calculating joint behaviour double b1 =1; double b2 =1; // Joint friction coefficent double w_s1 = tau_st1/b1; // Angular velocity double w_s2 = tau_st2/b2; // Angular velocity // Checking angle boundaries qRef1 = qRef1 +w_s1*1; // Calculating new angle of qRef1 in time step sampleTime if (qRef1 > 2*PI/3) { qRef1 = 2*PI/3; } else if (qRef1 < PI/6) { qRef1= PI/6; } qRef2 = qRef2 + w_s2*1; // Calculating new angle of qRef2 in time step sampleTime if (qRef2 > -PI/4) { qRef2 = -PI/4; } else if (qRef2 < -PI/2) { qRef2= -PI/2; } } } 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 double error1 = qRef1 - qMeas1; double error2 = qRef2 - qMeas2; static double errorIntegral1 = 0; static double errorIntegral2 = 0; static double errorPrev1 = error1; static double errorPrev2 = error2; static BiQuad LowPassFilter(0.0640, 0.1279, 0.0640, -1.1683, 0.4241); // Proportional part: double u_k1 = Kp1 * error1; double u_k2 = Kp2 * error2; //Integral part: errorIntegral1 = errorIntegral1 + error1 * sampleTime; double u_i1 = Ki1 * errorIntegral1; errorIntegral2 = errorIntegral2 + error2 * sampleTime; double u_i2 = Ki2 * errorIntegral2; // Derivative part double errorDerivative1 = (error1 - errorPrev1)/sampleTime; double filteredErrorDerivative1 = LowPassFilter.step(errorDerivative1); double u_d1 = Kd1 * filteredErrorDerivative1; errorPrev1 = error1; double errorDerivative2 = (error2 - errorPrev2)/sampleTime; double filteredErrorDerivative2 = LowPassFilter.step(errorDerivative2); double u_d2 = Kd2 * filteredErrorDerivative2; errorPrev2 = error2; // Sum all parts u1 = u_k1 + u_i1 + u_d1; u2 = u_k2 + u_i2 + u_d2; } } void controlMotor () // Control direction and speed { directionpin1 = u1 > 0.0f; // Either true or false 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 EMGwaiting(){ //Flashes LED led3 = !led3; waitingcounter++; } void EMGsampling(){ //Ticker function of EMG switch(processnow){ case 0: case 1: tot += abs(bqc.step(emg0.read())); break; case 2: case 3: tot += abs(bqc.step(emg2.read())); break; } counter++; if (counter >= samplecal){ tot = tot/ratio; switch(processnow){ case 0: armresttot = tot; pc.printf("<Result> Average area of arm signal in rest: %f.\r\n",armresttot); break; case 1: armflextot = tot; pc.printf("<Result>Average area of arm signal while flexing: %f.\r\n",armflextot); armcal = (armflextot + armresttot)/2.0; pc.printf("<Result> Calibration value for arms determined at %f.\r\n",armcal); break; case 2: legresttot = tot; pc.printf("<Result> Average area of leg signal in rest: %f.\r\n",legresttot); break; case 3: legflextot = tot; pc.printf("<Result> Average area of leg signal while flexing: %f.\r\n",legflextot); legcal = (legflextot + legresttot)/2; pc.printf("<Result> Calibration value for legs determined at %f.\r\n",legcal); break; } processnow++; tot = 0; counter = 0; } } void stateMachine () { switch (currentState) { case WaitState: if (stateChanged == true) { // Everything that needs to be done when this state is entered the first time led1 = 0; // Green led2 = 1; led3 = 1; u1 = 0; // Turn off all motors u2 = 0; u3 = 0; u4 = 0; stateChanged = false; } if (startButton.read() == false) { // When button is pressed, value is false // Everything that needs to be done when leaving this state led1 = 1; currentState = MotorCalState; stateChanged = true; } break; case MotorCalState: if (stateChanged == true) { led2 = 0; // Red u1 = 0.3; // Motor is set to 'low' value u2 = -0.3; stateTimer.reset(); stateTimer.start(); stateChanged = false; } // The robot can only go to the next state after 3s, when the motor velocities are close to 0 and the start button is pressed if (stateTimer >= 3.0f && fabs(measureVelocity(1)) < 100 && fabs(measureVelocity(2)) < 100 && startButton.read() == false) { led2 = 1; encoder1.reset(); // Reset encoder for the 0 position encoder2.reset(); currentState = EMGCalState; stateChanged = true; u1 = 0; // Turn off motors u2 = 0; } break; case EMGCalState: if (stateChanged == true) { led3 = 0; // Blue stateChanged = false; pc.printf("<----------Now entering EMG calibration state ---------->\r\n"); pc.printf("<Waiting> Get ready for calibration of resting arms.\r\n"); calTimer.attach(&EMGwaiting,0.5); //Wait while (waitingcounter < countermax) {} waitingcounter = 0; calTimer.detach(); pc.printf("<Measuring> Measuring arm signal at rest. Hold still.\r\n"); EMGsampler.attach(&EMGsampling,sampleTime); //Measure arms rest while (processnow == 0) {} EMGsampler.detach(); pc.printf("<Waiting> Get ready for calibration of flexing arms.\r\n"); calTimer.attach(&EMGwaiting,0.5); // Wait while (waitingcounter < countermax) {} waitingcounter = 0; calTimer.detach(); pc.printf("<Measuring> Measuring arm signal while flexing.\r\n"); EMGsampler.attach(&EMGsampling,sampleTime); //Measure arms flexing while (processnow == 1) {} EMGsampler.detach(); pc.printf("<Waiting> Get ready for calibration of resting legs.\r\n"); calTimer.attach(&EMGwaiting,0.5); //Wait while (waitingcounter < countermax) {} waitingcounter = 0; calTimer.detach(); pc.printf("<Measuring> Measuring leg signal at rest. Hold still.\r\n"); EMGsampler.attach(&EMGsampling,sampleTime); //Measure legs rest while (processnow == 2) {} EMGsampler.detach(); pc.printf("<Waiting> Get ready for calibration of flexing legs.\r\n"); calTimer.attach(&EMGwaiting,0.5); // Wait while (waitingcounter < countermax) {} waitingcounter = 0; calTimer.detach(); pc.printf("<Measuring> Measuring leg signal while flexing.\r\n"); EMGsampler.attach(&EMGsampling,sampleTime); //Measure legs flexing while (processnow == 3) {} EMGsampler.detach(); pc.printf("<Result> EMG calibrations complete.\r\n"); } if (stateTimer >= 3.0f) { led3 = 1; currentState = HomingState; stateChanged = true; } break; case HomingState: if (stateChanged == true) { led1 = 0; // Yellow led2 = 0; // qRef1 and qRef2 are set, so the motors will move to that position qRef1 = 60 * PI / 180; qRef2 = -90 * PI / 180; stateChanged = false; } if (startButton.read() == false) { //TODO: Also add position condition led1 = 1; led2 = 1; currentState = MovingState; stateChanged = true; } break; case MovingState: if (stateChanged == true) { led1 = 0; // White led2 = 0; led3 = 0; stateChanged = false; } if (gripperButton.read() == false) { led1 = 1; led2 = 1; led3 = 1; currentState = GrippingState; stateChanged = true; } break; case GrippingState: if (stateChanged == true) { led2 = 0; // Light blue led3 = 0; stateTimer.reset(); stateTimer.start(); stateChanged = false; } if (gripperMotorButton.read() == false) { // Gripper motor is activated 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; } // Due to the lack of buttons, we use a delay, so that when we don't immediately go to the next state when we enter this state 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; } break; case ScrewingState: if (stateChanged == true) { // Entry action: all the things you do once in this state; led1 = 0; // Pink led3 = 0; stateChanged = false; } if (gripperMotorButton.read() == false) { u4 = 0.3; // Screwing motor is activated u3 = -0.35; // Gripper motor is activated with a slightly higher output to make sure the gripper doesn't open if (directionSwitch == true) { // Screw } else { // Unscrew u4 = u4 * -1; u3 = u3 * -1; } } else { // If the button isn't pressed, turn off motors u4 = 0; u3 = 0; } if (startButton.read() == false) { led1 = 1; led3 = 1; u3 = 0; // Turn off motors u4 = 0; currentState = MovingState; stateChanged = true; } break; case FailureState: if (stateChanged == true) { // Entry action: all the things you do once in this state u1 = 0; // Turn off all motors u2 = 0; u3 = 0; u4 = 0; stateChanged = false; } static double blinkTimer2 = 0; if (blinkTimer2 >= 0.5) { // The red LED is flashing at 1 Hz led1 = !led1; blinkTimer2 = 0; } blinkTimer2 += sampleTime; break; } } void measureAll () { measurePosition(); measureEMG(); inverseKinematics(); } void mainLoop () // All the major functions are looped every 0.001s { measureAll(); stateMachine(); PID_controller(); controlMotor(); } int main() { startButton.mode(PullUp); // Makes the button active failureButton.mode(PullUp); gripperButton.mode(PullUp); directionSwitch.mode(PullUp); gripperMotorButton.mode(PullUp); pc.baud(115200); 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 while (true) { } }