Version for EMG Threshold finding
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of prog_BioRobotics_Group9_StampRobot by
Diff: main.cpp
- Revision:
- 23:3a4d5e19c20d
- Parent:
- 22:c679f753a8bd
- Child:
- 24:393da1cc1fa8
diff -r c679f753a8bd -r 3a4d5e19c20d main.cpp --- a/main.cpp Wed Oct 26 09:03:39 2016 +0000 +++ b/main.cpp Wed Oct 26 09:18:42 2016 +0000 @@ -16,7 +16,7 @@ - Set angle and length boundaries!! - Set robot constants (lengths etc.) - Set EMGgain and thresholds -- Add tower height to ReferencePosition_y and Position_y +- 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. @@ -45,29 +45,31 @@ //set constant or variable values float EMGgain = 1.0; -double DerivativeCounts; float x0 = 1.0; float L0 = 1.0; float L1 = 1.0; float L2 = 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 TowerHeight = 0.4; //height of motor axes above table surface! +float t_sample = 0.01; //seconds +const float maxStampDistance = 1.5; + float dx; float dy; -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 t_sample = 0.01; //seconds +double DerivativeCounts; //float referenceVelocity = 0; //float bqcDerivativeCounts = 0; 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; -const float maxStampDistance = 1.5; //set initial conditions float biceps_l = 0; float biceps_r = 0; float ReferencePosition_x = L2; -float ReferencePosition_y = L1; +float ReferencePosition_y = L1 + TowerHeight; float ReferencePosition_xnew = 0; float ReferencePosition_ynew = 0; float Position_x = 0.0; @@ -138,7 +140,7 @@ */ //get end effector position feedback with trigonometry Position_x = (L1*sin(q1) + L2*sin(q1+q2)); - Position_y = (L1*cos(q1) + L2*cos(q1+q2)); + Position_y = (L1*cos(q1) + L2*cos(q1+q2)) + TowerHeight; //float PositionVector = sqrt(pow(Position_x,2)+pow(Position_y,2)); //get velocity vector v = (Pe*- Pe) = [0; dx; dy] from EMG @@ -198,10 +200,10 @@ ReferencePosition_ynew = ReferencePosition_y; } - float ReferenceVector = sqrt(pow(ReferencePosition_x,2)+pow(ReferencePosition_y,2)); + float PointVector = sqrt(pow(PointPosition_x,2)+pow(PointPosition_y,2)); //check position boundaries - if (ReferenceVector > maxStampDistance){ + if (PointVector > maxStampDistance){ ReferencePosition_x = ReferencePosition_x; ReferencePosition_y = ReferencePosition_y; } @@ -214,10 +216,12 @@ } //calculate reference joint angles for the new reference position - float alpha = atan(ReferencePosition_y/ReferencePosition_x); - float beta = acos((L2*L2-L1*L1-pow(ReferenceVector,2))/(-2*L1*ReferenceVector)); + float PointPosition_x = ReferencePosition_x; + float PointPosition_y = ReferencePosition_y - TowerHeight; + float alpha = atan(PointPosition_y/PointPosition_x); + float beta = acos((L2*L2-L1*L1-pow(PointVector,2))/(-2*L1*PointVector)); q1_refOutNew = PI/2 - (alpha+beta); - q2_refOutNew = PI - asin(ReferenceVector*sin(beta)/L2); + q2_refOutNew = PI - asin(PointVector*sin(beta)/L2); //check angle boundaries if (q1_refOutNew > q1_refOutMin && q1_refOutMax < q1_refOutMax){ @@ -272,8 +276,6 @@ void FeedForwardControl1(float q1_ref, float q2_ref, float q1, float q2, float &motorValue1Out, float &motorValue2Out){ - //float Encoder1Position = counts1/resolution; //position in radians, encoder axis - //float Position1 = Encoder1Position*inverse_gear_ratio; //position in radians, motor axis // linear feedback control float q1_error = q1_ref - q1; //referencePosition1 - Position1; // proportional angular error in radians @@ -295,8 +297,8 @@ //scope.set(2,Ki); //scope.send(); - TotalError1 = q1_error * Kp + q1IntError * Ki + q1DerivativeError * Kd; //total controller output = motor input - TotalError2 = q2_error * Kp + q2IntError * Ki + q2DerivativeError * Kd; //total controller output = motor input + 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 (TotalError1 < TotalErrorMin) { TotalError1=0;