Version for EMG Threshold finding
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of prog_BioRobotics_Group9_StampRobot by
Diff: main.cpp
- Revision:
- 32:3b00eafcf168
- Parent:
- 30:676ecd59467a
diff -r 676ecd59467a -r 3b00eafcf168 main.cpp --- a/main.cpp Fri Oct 28 09:31:08 2016 +0000 +++ b/main.cpp Tue Nov 01 10:13:28 2016 +0000 @@ -41,7 +41,7 @@ //library settings Serial pc(USBTX,USBRX); Ticker MeasureTicker, BiQuadTicker; //, TimeTracker; // sampleT; -HIDScope scope(6); +//HIDScope scope(6); //initial values float dx; @@ -98,7 +98,7 @@ 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 MotorSpeedGain = 5.0; //to give the motor higher speeds +float MotorSpeedGain = 1.0; //to give the motor higher speeds float t_sample = 0.01; //seconds const float maxStampDistance = 1.5; @@ -288,7 +288,7 @@ // 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 - float Kp = 1; //potMeter2.read(); + float Kp = 1.0; //potMeter2.read(); float q1IntError = q1IntError + q1_error*t_sample; // integrated error in radians float q2IntError = q2IntError + q2_error*t_sample; // integrated error in radians @@ -334,10 +334,7 @@ motorValue1Out = MotorSpeedGain*motorValue1Small; motorValue2Out = MotorSpeedGain*motorValue2Small; - TotalError1_prev = TotalError1; - TotalError2_prev = TotalError2; - - + /* scope.set(0,q1_ref); scope.set(1,q1); scope.set(2,q2_ref); @@ -345,28 +342,25 @@ scope.set(4,motorValue1Out); scope.set(5,motorValue2Out); scope.send(); - + */ + /* pc.printf("error1: %f \r\n", q1_error); pc.printf("IntError1: %f \r\n", q1IntError); pc.printf("DerError1: %f \r\n", q1DerivativeError); pc.printf("error2: %f \r\n", q2_error); pc.printf("IntError2: %f \r\n", q2IntError); pc.printf("DerError2: %f \r\n", q2DerivativeError); - - /* + */ q1_error_prev = q1_error; q2_error_prev = q2_error; TotalError1_prev = TotalError1; TotalError1_prev = TotalError1; - */ - - /* - scope.set(0,q1); - scope.set(1,q2); - scope.set(2,biceps_l); - scope.set(3,biceps_r); - scope.send(); - */ + + pc.printf("TE1: %f ", TotalError1); + pc.printf("TE2: %f ", TotalError2); + pc.printf("M1: %f ", motorValue1Out); + pc.printf("M2: %f ", motorValue2Out); + } void SetMotor1(float motorValue1, float motorValue2)