Gerhard Berman
New reference frame: y=0 is now at table height.
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of
prog_forwardkin_feedback_copy
by 
Elfi Hofmeijer
Diff: main.cpp
- Revision:
- 44:43f200e04903
- Parent:
- 43:2b2e0bff0b39
diff -r 2b2e0bff0b39 -r 43f200e04903 main.cpp
--- a/main.cpp Sat Nov 05 13:54:09 2016 +0000
+++ b/main.cpp Sat Nov 05 16:00:26 2016 +0000
@@ -7,178 +7,137 @@
/*
THINGS TO CONSIDER
-- Line 234, 239: motor action of motor 1 is inverted because it is mounted
+- 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.
-- Line 244,257: motor values have been scaled down for safety at first test, restore
-after testing to get proper action.
-- Set angle and length boundaries!!
-- Set robot constants (lengths etc.)
-- Set EMGgain and thresholds
-- Add tower height to ReferencePosition_y and Position_y AND inverse kinematics calculation!
-- Add (lower) boundaries to TotalErrors
-- MotorGain could change due to arm weight!!
-- Arms should be placed manually into reference position.
+- Arms should be placed manually into reference position before resetting board.
*/
//set pins
-DigitalIn encoder1A (D13); //Channel A van Encoder 1
-DigitalIn encoder1B (D12); //Channel B van Encoder 1
-DigitalIn encoder2A (D11); //Channel A van Encoder 2, kan niet op D15
-DigitalIn encoder2B (D10); //Channel B van Encoder 2, kan niet op D14
-//DigitalOut led1 (D11);
-//DigitalOut led2 (D10);
-//AnalogIn potMeter1(A2);
-//AnalogIn potMeter2(A1);
+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);
-DigitalIn button1(D3);
-DigitalIn button2(D9);
-AnalogIn emg0( A0 );
-AnalogIn emg1( A1 );
-
DigitalOut ledGrn(LED_GREEN);
DigitalOut ledRed(LED_RED);
DigitalOut ledBlue(LED_BLUE);
//library settings
Serial pc(USBTX,USBRX);
-Ticker MeasureTicker, BiQuadTicker; //, TimeTracker; // sampleT;
HIDScope scope(6);
-//initial values
-float dx;
-float dy;
-double DerivativeCounts;
-//float referenceVelocity = 0;
-//float bqcDerivativeCounts = 0;
+//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 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 (VALUES HAVE TO BE CHANGED)
-//float x0 = 1.0;
-float L0 = 0.232;
-float L1 = 0.45;
-float L2 = 0.35;
-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
+//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
-
-//set initial conditions
-float biceps_l = 0;
-float biceps_r = 0;
-float ReferencePosition_x = 0.35; //L2;
-float ReferencePosition_y = L1 + TowerHeight - StampHeight;
-float ReferencePosition_xnew = 0.35; //L2;
+//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;
-float q1 = 0;
-float q2 = PI/2;
-float q1_ref = 0;
-float q2_ref = 0;
-float q1start = 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;
-float q12Encoder = 0;
+float q1Encoder = 0, q12Encoder = 0;
float q12Out = 0;
-float q1_error_prev = 0;
-float q2_error_prev = 0;
-float DerTotalError1 = 0;
-float DerTotalError2 = 0;
-float q1IntError = 0;
-float q2IntError = 0;
-float TotalError1_prev = 0;
-float TotalError2_prev = 0;
-
-float motorValue1 = 0.0;
-float motorValue2 = 0.0;
-int counts1 = 0;
-int counts2 = 0;
-int counts1Prev = 0;
-int counts2Prev = 0;
-double envelopeL = 0;
-double envelopeR = 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;
-//set constant or variable values (VALUES HAVE TO BE EDITED)
-int T=0; //EMG 'switch' variable
-double threshold_l=0.09; //left arm EMG threshold
-double threshold_r = 0.08; //right arm EMG threshold
-float EMGgain = 1.0;
-
-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 (motor value of 1)
-float Motor1ExtraGain = 1.0;
-float MotorMaxSpeed = 0.1; //define a maximum PWM speed for the motor
-float t_sample = 0.01; //seconds
-const float maxStampDistance = 0.7; //0.66;
-const float minStampDistance = 0.3;
-float Kp = 4.0;//potMeter2.read();
-float Ki = 0.04; //0.01*Kp; //potMeter2.read();
-float Kd = 0.02; //0.1;(unstable!) //0.05; //0.02; //0.04*Kp; //potMeter2.read();
-float N = 25; //N=1/Tf, Higher N is faster derivative action but more sensitive to noise.
+float motorValue1 = 0.0, motorValue2 = 0.0;
+int counts1 = 0, counts2 = 0;
+int counts1Prev = 0, counts2Prev = 0;
-float q1_refOutNew = 0;
-float q1_refOutMin = 0; //Physical min angle 0.00 radians + 0.1 rad
-float q1_refOutMax = 1.37; //Physical max angle 1.47 radians - 0.1 rad
-float q2_refOutNew = 0;
-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
-float TotalError1= 0;
-float TotalError2= 0;
-float TotalErrorMin= 0;
-
-//set BiQuad
-BiQuadChain bqc;
-BiQuad bq1(0.0186, 0.0743, 0.1114, 0.0743, 0.0186); //get numbers from butter filter MATLAB
-BiQuad bq2(1.0000, -1.5704, 1.2756, -0.4844, 0.0762);
+//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
-BiQuad pidf;
+//set BiQuad filters/filter chains
+
+BiQuad pidf; //PID Filter
-BiQuadChain bcq1R;
-BiQuadChain bcq2R;
-// Notch filter wo=50; bw=wo/35
-BiQuad bq1R(9.9110e-01,-1.6036e+00,9.9110e-01,-1.6036e+00,9.8221e-01);
-// High pass Butterworth filter 2nd order, Fc=10;
-BiQuad bq2R(9.1497e-01,-1.8299e+00,9.1497e-01,-1.8227e+00,8.3718e-01);
-// Low pass Butterworth filter 2nd order, Fc = 8;
-BiQuad bq3R(1.3487e-03,2.6974e-03,1.3487e-03,-1.8935e+00,8.9886e-01);
+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;
-BiQuadChain bcq2L;
-// Notch filter wo=50; bw=wo/35
-BiQuad bq1L(9.9110e-01,-1.6036e+00,9.9110e-01,-1.6036e+00,9.8221e-01);
-// High pass Butterworth filter 2nd order, Fc=10;
-BiQuad bq2L(9.1497e-01,-1.8299e+00,9.1497e-01,-1.8227e+00,8.3718e-01);
-// Low pass Butterworth filter 2nd order, Fc = 8;
-BiQuad bq3L(1.3487e-03,2.6974e-03,1.3487e-03,-1.8935e+00,8.9886e-01);
-// In the following: R is used for right arm, L is used for left arm!
-
-//set go-Ticker settings
-volatile bool MeasureTicker_go=false, BiQuadTicker_go=false, FeedbackTicker_go=false, TimeTracker_go=false; // sampleT_go=false;
-void MeasureTicker_act(){MeasureTicker_go=true;}; // Activates go-flags
-void BiQuadTicker_act(){BiQuadTicker_go=true;};
-void FeedbackTicker_act(){FeedbackTicker_go=true;};
-void TimeTracker_act(){TimeTracker_go=true;};
-//void sampleT_act(){sampleT_go=true;};
+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
+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
-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();
@@ -207,164 +166,119 @@
else{
//wait(0.2);
Tout = 0;
- }
-
-// scope.set(0, inLout);
-// scope.set(1, inRout);
-
+ }
}
-void GetReferenceKinematics1(float &q1Out, float &q2Out, float &q1_refOut, float &q2_refOut){
+//--------------------------------------------------------------------------------------------------------------------------
+//--------------------------------------------------------------------------------------------------------------------------
+
+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 in radians, encoder axis
- float Encoder2Position = -1*counts2/resolution; //NEGATIVE!
+ 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 in radians, motor axis
- q12Out = q12start + q12Encoder; //encoder 2 gives sum of both angles!
+ 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;
- float q2deg = q2Out*360/2/PI;
+ //float q1deg = q1Out*360/2/PI; //angle in degrees
+ //float q2deg = q2Out*360/2/PI; //angle in degrees
- /*
- //get end effector position feedback with Brockett
- float Position_x = ((L2 + x0)*(cos(q1)*cos(q2) - sin(q1)*sin(q2)) - L0*sin(q1) + (cos(q1)*sin(q2) + cos(q2)*sin(q1))*(L0 + L1) - cos(q1)*(L1*sin(q1) + L1*cos(q1)*sin(q2) - L1*cos(q2)*sin(q1)) - sin(q1)*(L1*cos(q1)*cos(q2) - L1*cos(q1) + L1*sin(q1)*sin(q2))); //calculate end effector x-position from motor angles with Brockett, rx
- float Position_y = (L0 - (L2 + x0)*(cos(q1)*sin(q2) + cos(q2)*sin(q1)) - L0*cos(q1) - cos(q1)*(L1*cos(q1)*cos(q2) - L1*cos(q1) + L1*sin(q1)*sin(q2)) + (cos(q1)*cos(q2) - sin(q1)*sin(q2))*(L0 + L1) + sin(q1)*(L1*sin(q1) + L1*cos(q1)*sin(q2) - L1*cos(q2)*sin(q1))); //calculate end effector y-position from motor angles with Brockett, ry
- */
- //get end effector position feedback with trigonometry
+ //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;
- //float PositionVector = sqrt(pow(Position_x,2)+pow(Position_y,2));
- /*
- if (Position_y < (0.5*TowerHeight)){
- wait(1.0);
- ReferencePosition_ynew = L1 + TowerHeight - StampHeight; //Reset vertical position after stamping
- }
- else{
- */
- //get velocity vector v = (Pe*- Pe) = [0; dx; dy] from EMG
- biceps_l = !button1.read() * EMGgain; //emg0.read(); //velocity or reference position change, EMG with a gain
- biceps_r = !button2.read() * EMGgain; //emg1.read();
+ //get reference position from EMG based T-switch values
if (T == -2){
- //both arms activated: stamp moves down
- //led1 = 1;
- //led2 = 1;
+ //both EMGs active: stamp moves down
ReferencePosition_xnew = ReferencePosition_x;
- ReferencePosition_ynew = ReferencePosition_y - 0.015; //ReferencePosition_y - dy_stampdown; //into stamping vertical position ~the stamp down action
-
+ ReferencePosition_ynew = ReferencePosition_y - 0.015;
}
else if (T==-1){
- //arm 1 activated, move left
- //led1 = 1;
- //led2 = 0;
- ReferencePosition_xnew = ReferencePosition_x - 0.0009; //biceps_l;
- ReferencePosition_ynew = y_stampup; //ReferencePosition_y;
- /*
- PositionError_x = ReferencePosition_x - Position_x; //Position error in dx,dy
- PositionError_y = ReferencePosition_y - Position_y; //Position error in dx,dy
-
- dx = PositionError_x;
- dy = PositionError_y;
- q1_dotOut = dy*(((x0 + L1*cos(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (x0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1))) - dx*(((L0 + L1*sin(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (L0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)));
- q2_dotOut = dy*((x0*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(L1*L1*pow(cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - ((x0 + L1*cos(q1))*(pow(L0,2) + pow(x0,2) + 1))/(pow(L1*cos(q1),2)) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - dx*((L0*(L0*L0+L1*sin(q1)*L0+x0*x0+L1*cos(q1)*x0+1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1))-((L0 + L1*sin(q1))*(L0*L0 + x0*x0 + 1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1)));
- */
+ //left EMG active, move stamp left
+ ReferencePosition_xnew = ReferencePosition_x - 0.0009;
+ ReferencePosition_ynew = y_stampup;
}
else if (T==1){
- //arm 1 activated, move right
- //led1 = 0;
- //led2 = 1;
- ReferencePosition_xnew = ReferencePosition_x + 0.0009; //biceps_r;
- ReferencePosition_ynew = y_stampup; //ReferencePosition_y;
- /*PositionError_x = ReferencePosition_x - Position_x; //Position error in dx,dy
- PositionError_y = ReferencePosition_y - Position_y; //Position error in dx,dy
-
- dx = PositionError_x;
- dy = PositionError_y;
- q1_dotOut = dy*(((x0 + L1*cos(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (x0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1))) - dx*(((L0 + L1*sin(q1))*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)) - (L0*(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + L0*L0 + x0*x0 + 2*L0*L1*sin(q1) + 2*L1*x0*cos(q1) + 1))/(pow(L1*cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*L1*L1*x0*cos(q1)*sin(q1)));
- q2_dotOut = dy*((x0*(L0*L0 + L1*sin(q1)*L0 + x0*x0 + L1*cos(q1)*x0 + 1))/(L1*L1*pow(cos(q1),2) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - ((x0 + L1*cos(q1))*(pow(L0,2) + pow(x0,2) + 1))/(pow(L1*cos(q1),2)) + pow(L1*sin(q1),2) + pow(L1*x0*sin(q1),2) + pow(L0*L1*cos(q1),2) - 2*L0*pow(L1,2)*x0*cos(q1)*sin(q1)) - dx*((L0*(L0*L0+L1*sin(q1)*L0+x0*x0+L1*cos(q1)*x0+1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1))-((L0 + L1*sin(q1))*(L0*L0 + x0*x0 + 1))/(pow(L1*cos(q1),2)+pow(L1*sin(q1),2)+pow(L1*x0*sin(q1),2)+pow(L0*L1*cos(q1),2)-2*L0*L1*L1*x0*cos(q1)*sin(q1)));
-*/
+ //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;
}
- else{ //T==0
- //led1 = 0;
- //led2 = 0;
- //ReferencePosition_xnew = ReferencePosition_x;
- ReferencePosition_ynew = y_stampup; //ReferencePosition_y; //
- }
- // }
- //check position boundaries
+ //check reference position boundaries
if (ReferencePosition_xnew > maxStampDistance){
- ReferencePosition_x = maxStampDistance; // - 0.1;
+ //target too far to be reached
+ ReferencePosition_x = maxStampDistance;
ReferencePosition_y = y_stampup;
- pc.printf("Target too far! \r\n");
+ //pc.printf("Target too far! \r\n");
}
else if (ReferencePosition_xnew < minStampDistance){
- ReferencePosition_x = minStampDistance; // + 0.1;
+ //target too close
+ ReferencePosition_x = minStampDistance;
ReferencePosition_y = y_stampup;
- pc.printf("Target too close! \r\n");
+ //pc.printf("Target too close! \r\n");
}
else if (ReferencePosition_ynew < y_stampdown){
- ReferencePosition_x = ReferencePosition_xnew; // + 0.1;
+ //target under table surface
+ ReferencePosition_x = ReferencePosition_xnew;
ReferencePosition_y = y_stampdown;
- pc.printf("Target too close! \r\n");
+ //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));
- //calculate reference joint angles for the new reference position
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 angle boundaries
+ //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");
+ //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");
+ //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;
}
- //update joint angles
- //q1Out = q1Out + q1_dotOut; //in radians
- //q2Out = q2Out + q2_dotOut;
-
- pc.baud(115200);
- pc.printf("posX: %f ",Position_x);
- pc.printf("posY: %f ",Position_y);
- pc.printf("refX: %f ",ReferencePosition_xnew);
- pc.printf("refY: %f ",ReferencePosition_ynew);
- pc.printf("q1: %f ", q1Out);
- pc.printf("q1ref: %f ", q1_refOut);
- pc.printf("q2: %f ", q2Out);
- pc.printf("q2ref: %f ", q2_refOut);
- pc.printf("q1deg: %f ", q1deg);
- pc.printf("q2deg: %f \r\n", q2deg);
scope.set(0, ReferencePosition_xnew);
scope.set(1, ReferencePosition_ynew);
@@ -372,137 +286,66 @@
scope.set(3, envelopeR);
scope.set(4, T);
scope.send();
- /*
- pc.printf("dx: %f \r\n", dx);
- pc.printf("dy: %f \r\n", dy);
- pc.printf("q1: %f \r\n", q1Out);
- pc.printf("q1_dot: %f \r\n", q1_dotOut);
- pc.printf("q2: %f \r\n", q2Out);
- pc.printf("q2_dot: %f \r\n", q2_dotOut);
+}
+
+//--------------------------------------------------------------------------------------------------------------------------
+//--------------------------------------------------------------------------------------------------------------------------
+
+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
- pc.printf("Counts1: %f \r\n", counts1);
- pc.printf("Encoder1: %f \r\n", Encoder1Position);
- pc.printf("Motor1: %f \r\n", q1Out);
- pc.printf("Counts2: %f \r\n", counts2);
- pc.printf("Encoder2: %f \r\n", Encoder2Position);
- pc.printf("Motor2: %f \r\n", q2Out);
- */
+ //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
- }
-
-
-void FeedbackControl1(float q1_ref, float q2_ref, float q1, float q2, float &motorValue1Out, float &motorValue2Out){
-
- // linear feedback control
- float q1_error = q1_ref - q1; //referencePosition1 - Position1; // proportional angular error in radians
- float q2_error = q2_ref - q2; //referencePosition1 - Position1; // proportional angular error in radians
-
+ //PIDF total error output
float TotalError1 = pidf.step(q1_error);
float TotalError2 = pidf.step(q2_error);
- /*
- float q1IntError = q1IntError + q1_error*t_sample; // integrated error in radians
- float q2IntError = q2IntError + q2_error*t_sample; // integrated error in radians
- //float maxKi = 0.2;
- float Ki = 0.04*Kp; //0.01*Kp; //potMeter2.read();
-
- float q1DerivativeError = (q1_error - q1_error_prev)/t_sample; // derivative of error in radians
- float q2DerivativeError = (q2_error_prev + q2_error)/t_sample; // derivative of error in radians
- //float maxKd = 0.2;
- float Kd = 0.0; //0.04*Kp; //potMeter2.read();
-
- //scope.set(0,referencePosition1);
- //scope.set(1,Position1);
- //scope.set(2,Ki);
- //scope.send();
-
- TotalError1 = q1_error * Kp + q1IntError * Ki + q1DerivativeError * Kd; //total controller output in radians = motor input
- TotalError2 = q2_error * Kp + q2IntError * Ki + q2DerivativeError * Kd; //total controller output in radians = motor input
- */
- /*
- if (fabs(TotalError1) < TotalErrorMin) {
- TotalError1=0;
- }
- else {
- TotalError1=TotalError1;
- }
- if (fabs(TotalError2) < TotalErrorMin) {
- TotalError2=0;
- }
- else {
- TotalError2=TotalError2;
- }
- */
- /*
- DerTotalError1 = (TotalError1 - TotalError1_prev)/t_sample;
- DerTotalError1 = (TotalError1 - TotalError1_prev)/t_sample;
- motorValue1Out = DerTotalError1/MotorGain;
- motorValue2Out = DerTotalError2/MotorGain;
- */
-
- motorValue1Out = Motor1ExtraGain*(TotalError1/MotorGain);
+ //calculate motor values from the total errors
+ motorValue1Out = TotalError1/MotorGain;
motorValue2Out = TotalError2/MotorGain;
- /*
- scope.set(0,q1_ref);
- scope.set(1,q1);
- scope.set(2,q2_ref);
- scope.set(3,q2);
- scope.set(4,motorValue1Out);
- scope.set(5,motorValue2Out);
- scope.send();
- */
- /*
- pc.printf("E1: %f ", q1_error);
- pc.printf("IE1: %f ", q1IntError);
- pc.printf("DE1: %f ", q2DerivativeError);
- pc.printf("E2: %f ", q2_error);
- pc.printf("IE2: %f ", q2IntError);
- pc.printf("DE2: %f ", q2DerivativeError);
- */
- //pc.printf("TE1: %f ", TotalError1);
- //pc.printf("TE2: %f ", TotalError2);
- pc.printf("M1: %f \r\n", motorValue1Out);
- pc.printf("M2: %f \r\n", motorValue2Out);
-
+ //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)
{
- // Given -1<=motorValue<=1, this sets the PWM and direction
- // bits for motor 1. Positive value makes motor rotating
- // clockwise. motorValues outside range are truncated to
- // within range
+//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; //inverted due to opposite (to other motor) build-up in tower
- //led1=1;
- //led2=0;
+ if (motorValue1 >=0){
+ //clockwise rotation
+ motor1DirectionPin=cw;
}
- else //counterclockwise rotation
- {motor1DirectionPin=ccw; //inverted due to opposite (to other motor) build-up in tower
- //led1=0;
- //led2=1;
+ else{
+ //counterclockwise rotation
+ motor1DirectionPin=ccw;
}
if (fabs(motorValue1)>MotorMaxSpeed){
- motor1MagnitudePin = MotorMaxSpeed;
+ motor1MagnitudePin = MotorMaxSpeed; //motor values truncated
}
else{
- motor1MagnitudePin = fabs(motorValue1); //fabs(motorValue1);
+ motor1MagnitudePin = fabs(motorValue1);
}
//control motor 2
- if (motorValue2 >=0) //clockwise rotation
- {motor2DirectionPin=cw; //action is ccw, due to faulty motor2DirectionPin (inverted)
- //led1=1;
- //led2=0;
+ if (motorValue2 >=0){
+ //counterclockwise rotation due to inverse buildup in tower
+ motor2DirectionPin=cw; //action is ccw, due to faulty motor2DirectionPin (inverted)
}
- else //counterclockwise rotation
- {motor2DirectionPin=ccw; //action is cw, 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;
}
@@ -512,83 +355,56 @@
else{
motor2MagnitudePin = fabs(motorValue2);
}
- float ReadDir1 = motor1DirectionPin.read();
- float ReadDir2 = motor2DirectionPin.read();
- pc.printf("M1 dir: %f \r\n", ReadDir1);
- pc.printf("M2 dir: %f \r\n", ReadDir2);
}
+//--------------------------------------------------------------------------------------------------------------------------
+//--------------------------------------------------------------------------------------------------------------------------
+
void MeasureAndControl()
{
- // This function measures the EMG of both arms, calculates via IK what
- // the joint positions should be, and controls the motor with
- // a Feedback controller. This is called from a Ticker.
+// 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);
}
-void TimeTrackerF(){
- //wait(1);
- //float Potmeter1 = potMeter1.read();
- //float referencePosition1 = GetReferencePosition();
- //pc.printf("TTReference Position: %d rad \r\n", referencePosition1);
- //pc.printf("TTPotmeter1, for refpos: %f \r\n", Potmeter1);
- //pc.printf("TTPotmeter2, Kp: %f \r\n", Potmeter2);
- //pc.printf("TTCounts: %i \r\n", counts1);
-}
-
-/*
-void BiQuadFilter(){ //this function creates a BiQuad filter for the DerivativeCounts
- //double in=DerivativeCounts();
- bqcDerivativeCounts=bqc.step(DerivativeCounts);
- //return(bqcDerivativeCounts);
- }
-*/
+//--------------------------------------------------------------------------------------------------------------------------
+//--------------------------------------------------------------------------------------------------------------------------
int main()
{
- //Initialize
- //int led1val = led1.read();
- //int led2val = led2.read();
- pc.baud(115200);
- pc.printf("Test putty IK");
- ledRed=1;
- ledBlue=1;
- ledRed=0; //red
+ //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);
-
- counts1 = Encoder1.getPulses(); // gives position of encoder
- counts2 = Encoder2.getPulses(); // gives position of encoder
- wait(20.0);
- MeasureTicker.attach(&MeasureTicker_act, 0.002); //0.1f);
- //bqc.add(&bq1).add(&bq2); //set BiQuad chain
- pidf.PIDF( Kp, Ki, Kd, N, t_sample ); //set PID filter
+ 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();
- counts1 = Encoder1.getPulses(); // gives position of encoder
- counts2 = Encoder2.getPulses(); // gives position of encoder
- //pc.printf("counts1: %i ", counts1);
- //pc.printf("counts2: %i \r\n", counts2);
+ MeasureAndControl(); //execute whole MeasureAndControl function
+ counts1 = Encoder1.getPulses(); //get encoder counts again
+ counts2 = Encoder2.getPulses(); //get encoder counts again
ledBlue = 1;
ledGrn = 0;
}
-/*
- if (BiQuadTicker_go){
- BiQuadTicker_go=false;
- BiQuadFilter();
- }
- */
}
}
\ No newline at end of file