Final version of BioRobotics program for Stamp Robot of Group 9, Biomedical Engineering at University of Twente. Script has been cleaned from any unused stuff, comments are unified in style.
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of prog_forwardkin_feedback_copy3 by
main.cpp
- Committer:
- GerhardBerman
- Date:
- 2016-11-07
- Revision:
- 48:73a5f7f62aec
- Parent:
- 47:13b4a318a3d0
File content as of revision 48:73a5f7f62aec:
#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 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 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; bandwidth=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; bandwidth=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 to acquire an EMG envelope 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(); //left biceps filtered EMG signal with a gain double biceps_r = (double) envelopeRout * EMGgain; //right biceps filtered EMG signal with a gain 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 joint angles from encoder, calculates reference position from //T-switch value, converts reference position to reference joint angles and checks boundaries. //get joint positions q feedback from encoder float Encoder1Position = counts1/resolution; //angular encoder position (rad) 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 motor axis position (rad) 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) } 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; //set BiQuad chains bcq1R.add(&bq1R).add(&bq2R); 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 MeasureAndControl counts1 = Encoder1.getPulses(); //get encoder counts again counts2 = Encoder2.getPulses(); //get encoder counts again ledBlue = 1; ledGrn = 0; } } }