Version for EMG Threshold finding
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of prog_BioRobotics_Group9_StampRobot by
main.cpp
- Committer:
- GerhardBerman
- Date:
- 2016-11-05
- Revision:
- 45:0f91abd76b93
- Parent:
- 44:43f200e04903
- Child:
- 46:4af2f01da9f3
File content as of revision 45:0f91abd76b93:
#include "mbed.h" #include <math.h> #include "MODSERIAL.h" #include "QEI.h" #include "HIDScope.h" #include "BiQuad.h" /* THINGS TO CONSIDER - Motor action of motor 1 is inverted because it is mounted opposite to motor 2 in the tower. Check if the clockwise directions of the motors correspond to the positive q1, q2-directions (both counterclockwise) in the original IK-sketch. - Robot arms should be placed manually into reference position before resetting board. */ //set pins DigitalIn encoder1A (D13); //Channel A from Encoder 1 DigitalIn encoder1B (D12); //Channel B from Encoder 1 DigitalIn encoder2A (D11); //Channel A from Encoder 2 DigitalIn encoder2B (D10); //Channel B from Encoder 2 AnalogIn emg0( A0 ); AnalogIn emg1( A1 ); DigitalIn button1(D3); DigitalIn button2(D9); DigitalOut motor1DirectionPin(D7); PwmOut motor1MagnitudePin(D6); DigitalOut motor2DirectionPin(D4); PwmOut motor2MagnitudePin(D5); DigitalOut ledGrn(LED_GREEN); DigitalOut ledRed(LED_RED); DigitalOut ledBlue(LED_BLUE); //library settings Serial pc(USBTX,USBRX); HIDScope scope(6); //go-Ticker settings Ticker MeasureTicker; volatile bool MeasureTicker_go=false; void MeasureTicker_act(){MeasureTicker_go=true;}; // Activates go-flag //constant values const float PI = 3.141592653589793; const int cw = 0; //values for cw and ccw are inverted! cw=0 and ccw=1 const int ccw = 1; double threshold_l=0.09; //left arm EMG threshold double threshold_r = 0.08; //right arm EMG threshold float EMGgain = 1.0; //set lengths //float L0 = 0.232; //height of motor axes above table surface float L1 = 0.45; //length of proximal arm float L2 = 0.35; //length of distal arm float TowerHeight = 0.232; //height of motor axes above table surface float StampHeight = 0.056; // height of end effector float y_stampup = 0.1; //height stamp while not stamping: 10cm above table surface float y_stampdown = -0.04; //height stamp while stamping: at table surface //float dy_stampdown = 2.0; //0.05; //5 cm movement downward to stamp float MotorGain = 8.4; // rad/s for PWM, is max motor speed float MotorMaxSpeed = 0.1; //define a maximum PWM speed for the motor float t_sample = 0.002; //seconds const float maxStampDistance = 0.7; const float minStampDistance = 0.3; float Kp = 4.0; //proportional controller constant float Ki = 0.04; //integrative controller constant float Kd = 0.02; //derivative controller constant float N = 25; //PIDF filter constant //(Higher N is faster derivative action but more sensitive to noise) //set initial conditions for inputs and positions float biceps_l = 0, biceps_r = 0; double envelopeL = 0, envelopeR = 0; int T=0; //EMG 'switch' variable float ReferencePosition_x = 0.35; //starting position for x reference position float ReferencePosition_y = L1 + TowerHeight - StampHeight; //starting position for y reference position float ReferencePosition_xnew = 0.35; float ReferencePosition_ynew = L1 + TowerHeight - StampHeight; float Position_x = 0.0; float Position_y = 0.0; //set initial conditions for angles, errors and motor values float q1 = 0,q2 = PI/2; float q1_ref = 0, q2_ref = 0; float q1start = 0; float q12start = PI/2; float q1Encoder = 0, q12Encoder = 0; float q12Out = 0; float q1_error_prev = 0, q2_error_prev = 0; float DerTotalError1 = 0, DerTotalError2 = 0; float q1IntError = 0, q2IntError = 0; float TotalError1_prev = 0, TotalError2_prev = 0; float TotalError1= 0, TotalError2= 0; //float TotalErrorMin= 0; float motorValue1 = 0.0, motorValue2 = 0.0; int counts1 = 0, counts2 = 0; int counts1Prev = 0, counts2Prev = 0; //set reference angle boundaries float q1_refOutNew = 0, q2_refOutNew = 0; float q1_refOutMin = 0; //Physical min angle 0.00 radians float q1_refOutMax = 1.37; //Physical max angle 1.47 radians - 0.1 rad float q2_refOutMin = 0.91; //Physical min angle 0.81 radians + 0.1 rad float q2_refOutMax = 2.07; //Physical max angle 2.17 radians - 0.1 rad //set BiQuad filters/filter chains BiQuad pidf; //PID Filter BiQuadChain bcq1R; //Right EMG filter chain 1: notch filter+highpass BiQuadChain bcq2R; //Right EMG filter chain 2: lowpass BiQuad bq1R(9.9110e-01,-1.6036e+00,9.9110e-01,-1.6036e+00,9.8221e-01); // Notch filter wo=50; bw=wo/35 BiQuad bq2R(9.1497e-01,-1.8299e+00,9.1497e-01,-1.8227e+00,8.3718e-01); // High pass Butterworth filter 2nd order, Fc=10; BiQuad bq3R(1.3487e-03,2.6974e-03,1.3487e-03,-1.8935e+00,8.9886e-01); // Low pass Butterworth filter 2nd order, Fc = 8; BiQuadChain bcq1L; //Left EMG filter chain 1: notch filter+highpass BiQuadChain bcq2L; //Left EMG filter chain 2: lowpass BiQuad bq1L(9.9110e-01,-1.6036e+00,9.9110e-01,-1.6036e+00,9.8221e-01); // Notch filter wo=50; bw=wo/35 BiQuad bq2L(9.1497e-01,-1.8299e+00,9.1497e-01,-1.8227e+00,8.3718e-01); // High pass Butterworth filter 2nd order, Fc=10; BiQuad bq3L(1.3487e-03,2.6974e-03,1.3487e-03,-1.8935e+00,8.9886e-01); // Low pass Butterworth filter 2nd order, Fc = 8; // In the following: R is used for right arm EMG, L is used for left arm EMG //define encoder counts and degrees QEI Encoder1(D12, D13, NC, 32); // turns on encoder QEI Encoder2(D10, D11, NC, 32); // turns on encoder const int counts_per_revolution = 4200; //counts per motor axis revolution const int inverse_gear_ratio = 131; const float resolution = counts_per_revolution/(2*PI/inverse_gear_ratio); //87567.0496892 counts per radian, encoder axis //-------------------------------------------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------------------------------------------- void FilteredSample(int &Tout, double &envelopeLout, double &envelopeRout) { //This function reads EMG, filters it and generates a T-switch value which specifies the movement of the robot double inLout = emg0.read(); double inRout = emg1.read(); double outRfilter1 = bcq1R.step(inRout); double outRrect= fabs(outRfilter1); envelopeRout = bcq2R.step(outRrect); double outLfilter1 = bcq1L.step(inLout); double outLrect = fabs(outLfilter1); envelopeLout = bcq2L.step(outLrect); double biceps_l = (double) envelopeLout * EMGgain; //emg0.read(); //velocity or reference position change, EMG with a gain double biceps_r = (double) envelopeRout * EMGgain; //emg1.read(); if (biceps_l > threshold_l && biceps_r > threshold_r){ //both arms activated: stamp moves down Tout = -2; } else if (biceps_l > threshold_l && biceps_r <= threshold_r){ //arm 1 activated, move left Tout = -1; } else if (biceps_l <= threshold_l && biceps_r > threshold_r){ //arm 1 activated, move right Tout = 1; } else{ //wait(0.2); Tout = 0; } } //-------------------------------------------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------------------------------------------- void GetReferenceKinematics1(float &q1Out, float &q2Out, float &q1_refOut, float &q2_refOut) { //This function reads current position from encoder, calculates reference position from T-switch value, //converts reference position to reference angles and checks boundaries //get joint positions q feedback from encoder float Encoder1Position = counts1/resolution; //angular position of encoder in radians float Encoder2Position = -1*counts2/resolution; //NEGATIVE due to opposite build up in tower q1Encoder = Encoder1Position*inverse_gear_ratio; q12Encoder = Encoder2Position*inverse_gear_ratio; q1Out = q1start + q1Encoder; //angular position of motor axis in radians q12Out = q12start + q12Encoder; //encoder 2 gives sum of q1 and q2! q2Out = q12Out - q1Out; //float q1deg = q1Out*360/2/PI; //angle in degrees //float q2deg = q2Out*360/2/PI; //angle in degrees //get end effector position with trigonometry Position_x = (L1*sin(q1) + L2*sin(q1+q2)); Position_y = (L1*cos(q1) + L2*cos(q1+q2)) + TowerHeight - StampHeight; //get reference position from EMG based T-switch values if (T == -2){ //both EMGs active: stamp moves down ReferencePosition_xnew = ReferencePosition_x; ReferencePosition_ynew = ReferencePosition_y - 0.015; } else if (T==-1){ //left EMG active, move stamp left ReferencePosition_xnew = ReferencePosition_x - 0.0009; ReferencePosition_ynew = y_stampup; } else if (T==1){ //right EMG active, move stamp right ReferencePosition_xnew = ReferencePosition_x + 0.0009; ReferencePosition_ynew = y_stampup; } else{ //T==0 //no EMG active, no x-movement, y-position restored to non-stamping position ReferencePosition_ynew = y_stampup; } //check reference position boundaries if (ReferencePosition_xnew > maxStampDistance){ //target too far to be reached ReferencePosition_x = maxStampDistance; ReferencePosition_y = y_stampup; //pc.printf("Target too far! \r\n"); } else if (ReferencePosition_xnew < minStampDistance){ //target too close ReferencePosition_x = minStampDistance; ReferencePosition_y = y_stampup; //pc.printf("Target too close! \r\n"); } else if (ReferencePosition_ynew < y_stampdown){ //target under table surface ReferencePosition_x = ReferencePosition_xnew; ReferencePosition_y = y_stampdown; //pc.printf("Target too low! \r\n"); } else { //target within reach ReferencePosition_x = ReferencePosition_xnew; ReferencePosition_y = ReferencePosition_ynew; } //calculate reference joint angles for the new reference position float PointPositionArm2_x = ReferencePosition_x; float PointPositionArm2_y = ReferencePosition_y - TowerHeight + StampHeight; float PointVectorArm2 = sqrt(pow(PointPositionArm2_x,2)+pow(PointPositionArm2_y,2)); float alpha = atan(PointPositionArm2_y/PointPositionArm2_x); float beta = acos((L2*L2-L1*L1-pow(PointVectorArm2,2))/(-2*L1*PointVectorArm2)); q1_refOutNew = PI/2 - (alpha+beta); q2_refOutNew = PI - asin(PointVectorArm2*sin(beta)/L2); //check reference angle boundaries if (q1_refOutNew < q1_refOutMin){ //new q1_ref too small q1_refOut = q1_refOutMin; //pc.printf("\r\n Under q1 angle boundaries\r\n"); } else if (q1_refOutNew > q1_refOutMax){ //new q1_ref too large q1_refOut = q1_refOutMax; //pc.printf("\r\n Above q1 angle boundaries\r\n"); } else { //new q1_ref within reach q1_refOut = q1_refOutNew; } if (q2_refOutNew < q2_refOutMin){ //new q2_ref too small q2_refOut = q2_refOutMin; pc.printf("\r\n Under q2 angle boundaries"); } else if (q2_refOutNew > q2_refOutMax){ //new q2_ref too large q2_refOut = q2_refOutMax; pc.printf("\r\n Above q2 angle boundaries"); } else { //new q2_ref within reach q2_refOut = q2_refOutNew; } scope.set(0, ReferencePosition_xnew); scope.set(1, ReferencePosition_ynew); scope.set(2, envelopeL); scope.set(3, envelopeR); scope.set(4, T); scope.send(); } //-------------------------------------------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------------------------------------------- void FeedbackControl1(float q1_ref, float q2_ref, float q1, float q2, float &motorValue1Out, float &motorValue2Out) { //This function calculates the error between angles and refernce angles, and provides motor values via a PIDF controller //calculate error values float q1_error = q1_ref - q1; // proportional angular error in radians float q2_error = q2_ref - q2; // proportional angular error in radians //PIDF total error output float TotalError1 = pidf.step(q1_error); float TotalError2 = pidf.step(q2_error); //calculate motor values from the total errors motorValue1Out = TotalError1/MotorGain; motorValue2Out = TotalError2/MotorGain; //prepare for next ticker cycle q1_error_prev = q1_error; q2_error_prev = q2_error; TotalError1_prev = TotalError1; TotalError2_prev = TotalError2; } //-------------------------------------------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------------------------------------------- void SetMotor1(float motorValue1, float motorValue2) { //This function sets the PWM and direction bits for the motors. //motorValues outside range are truncated to within range. //control motor 1 if (motorValue1 >=0){ //clockwise rotation motor1DirectionPin=cw; } else{ //counterclockwise rotation motor1DirectionPin=ccw; } if (fabs(motorValue1)>MotorMaxSpeed){ motor1MagnitudePin = MotorMaxSpeed; //motor values truncated } else{ motor1MagnitudePin = fabs(motorValue1); } //control motor 2 if (motorValue2 >=0){ //counterclockwise rotation due to inverse buildup in tower motor2DirectionPin=cw; //action is ccw, due to faulty motor2DirectionPin (inverted) } else{ //clockwise rotation due to inverse buildup in tower motor2DirectionPin=ccw; //action is cw, due to faulty motor2DirectionPin (inverted) //led1=0; //led2=1; } if (fabs(motorValue2)>MotorMaxSpeed){ motor2MagnitudePin = MotorMaxSpeed; } else{ motor2MagnitudePin = fabs(motorValue2); } } //-------------------------------------------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------------------------------------------- void MeasureAndControl() { // This function measures the EMG of both arms, calculates via inverse kinematics // what the joint reference positions should be, and controls the motor with // a Feedback controller. This is called from a Ticker. FilteredSample(T, envelopeL, envelopeR); GetReferenceKinematics1(q1, q2, q1_ref, q2_ref); FeedbackControl1( q1_ref, q2_ref, q1, q2, motorValue1, motorValue2); SetMotor1(motorValue1, motorValue2); } //-------------------------------------------------------------------------------------------------------------------------- //-------------------------------------------------------------------------------------------------------------------------- int main() { //pc.baud(115200); //pc.printf("Test putty IK"); ledRed=1; ledBlue=1; ledRed=0; bcq1R.add(&bq1R).add(&bq2R); //set BiQuad chains bcq2R.add(&bq3R); bcq1L.add(&bq1L).add(&bq2L); bcq2L.add(&bq3L); pidf.PIDF( Kp, Ki, Kd, N, t_sample ); //set PID filter //start up encoders counts1 = Encoder1.getPulses(); //gives position of encoder in counts counts2 = Encoder2.getPulses(); //gives position of encoder in counts wait(20.0); //time to sart up HIDScope and EMG MeasureTicker.attach(&MeasureTicker_act, 0.002); //initialize MeasureTicker, 500 Hz while(1) { if (MeasureTicker_go){ MeasureTicker_go=false; ledGrn = 1; ledBlue = 0; MeasureAndControl(); //execute whole MeasureAndControl function counts1 = Encoder1.getPulses(); //get encoder counts again counts2 = Encoder2.getPulses(); //get encoder counts again ledBlue = 1; ledGrn = 0; } } }