Version for EMG Threshold finding
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of prog_BioRobotics_Group9_StampRobot by
Diff: main.cpp
- Revision:
- 24:393da1cc1fa8
- Parent:
- 23:3a4d5e19c20d
- Child:
- 25:596255732b65
diff -r 3a4d5e19c20d -r 393da1cc1fa8 main.cpp --- a/main.cpp Wed Oct 26 09:18:42 2016 +0000 +++ b/main.cpp Wed Oct 26 09:24:38 2016 +0000 @@ -200,6 +200,8 @@ ReferencePosition_ynew = ReferencePosition_y; } + float PointPosition_x = ReferencePosition_x; + float PointPosition_y = ReferencePosition_y - TowerHeight; float PointVector = sqrt(pow(PointPosition_x,2)+pow(PointPosition_y,2)); //check position boundaries @@ -216,8 +218,6 @@ } //calculate reference joint angles for the new reference position - 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); @@ -275,7 +275,7 @@ } -void FeedForwardControl1(float q1_ref, float q2_ref, float q1, float q2, float &motorValue1Out, float &motorValue2Out){ +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 @@ -383,10 +383,10 @@ void MeasureAndControl() { // This function measures the EMG of both arms, calculates via IK what - // the joint speeds should be, and controls the motor with - // a Feedforward controller. This is called from a Ticker. + // the joint positions should be, and controls the motor with + // a Feedback controller. This is called from a Ticker. GetReferenceKinematics1(q1, q2, q1_ref, q2_ref); - FeedForwardControl1( q1_ref, q2_ref, q1, q2, motorValue1, motorValue2); + FeedbackControl1( q1_ref, q2_ref, q1, q2, motorValue1, motorValue2); SetMotor1(motorValue1, motorValue2); }