Version for EMG Threshold finding
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of prog_BioRobotics_Group9_StampRobot by
Diff: main.cpp
- Revision:
- 6:3c4f3f2ce54f
- Parent:
- 5:37e230689418
- Child:
- 7:2f74dfd1d411
diff -r 37e230689418 -r 3c4f3f2ce54f main.cpp --- a/main.cpp Mon Oct 17 13:10:20 2016 +0000 +++ b/main.cpp Mon Oct 17 14:34:25 2016 +0000 @@ -19,7 +19,7 @@ //set settings Serial pc(USBTX,USBRX); Ticker MeasureTicker, BiQuadTicker; //, TimeTracker; // sampleT; -HIDScope scope(2); +HIDScope scope(3); //set datatypes int counts = 0; @@ -76,21 +76,26 @@ float EncoderPosition = counts/resolution; //position in radians, encoder axis float Position = EncoderPosition*inverse_gear_ratio; //position in radians, motor axis // linear feedback control - scope.set(0,referencePosition); - scope.set(1,Position); - scope.send(); - float error = referencePosition - Position; // 'error' in radians + + float error = referencePosition - Position; // proportional error in radians float Kp = 1; //potMeter2.read(); - float IntError = IntError + error*t_sample; + float IntError = IntError + error*t_sample; // integrated error in radians float maxKi = 0.2; - float Ki = potMeter2.read()*maxKi; + float Ki = potMeter2.read(); //*maxKi; - //float DerivativeError = (error_prev + error)/t_sample; + /* + float DerivativeError = (error_prev + error)/t_sample; // derivative of error in radians float maxKd = 0.2; - //float Kd = potMeter2.read()*maxKd; + float Kd = potMeter2.read()*maxKd; + */ - float motorValue = error * Kp + IntError * Ki; //+ DerivativeError * Kd; + scope.set(0,referencePosition); + scope.set(1,Position); + scope.set(2,Ki); + scope.send(); + + float motorValue = error * Kp + IntError * Ki; // + DerivativeError * Kd; //total controller output = motor input pc.printf("Motor Axis Position: %f rad \r\n", Position); pc.printf("Counts encoder: %i rad \r\n", counts); pc.printf("Kp: %f \r\n", Kp); @@ -139,53 +144,13 @@ pc.printf("TTCounts: %i \r\n", counts); } /* -void sample() -{ - int countsPrev = 0; - QEI Encoder(D12, D13, NC, 32); - counts = Encoder.getPulses(); // gives position - //scope.set(0,counts); - DerivativeCounts = (counts-countsPrev)/0.001; - //scope.set(1,DerivativeCounts); - countsPrev = counts; - //scope.send(); - pc.printf("Counts: %i rad/s \r\n", counts); - pc.printf("Derivative Counts: %d rad/s \r\n", DerivativeCounts); -} - void BiQuadFilter(){ //this function creates a BiQuad filter for the DerivativeCounts //double in=DerivativeCounts(); bqcDerivativeCounts=bqc.step(DerivativeCounts); //return(bqcDerivativeCounts); } - -void MeasureP(){ - double ref_position = Potmeter1; //reference position from potmeter - int counts = Encoder.getPulses(); // gives position - double position = counts/resolution; //position in radians - double rotation = ref_position-position; //rotation is 'position error' in radians - double movement = rotation/(2*PI); //movement in rotations - double Kp = Potmeter2; - } - -double P(double rotation, double Kp){ - double P_output = Kp*movement; - return P_output; - } - -void MotorController(){ - double output = P(rotation, Kp); - - if(rotation>0){ - motor1DirectionPin.write(cw); - motor1MagnitudePin.write(output); - } - if(rotation<0){ - motor1DirectionPin.write(ccw); - motor1MagnitudePin.write(-output); - } - } -*/ + */ + int main() { //Initialize @@ -193,15 +158,9 @@ led2=1; pc.baud(115200); pc.printf("Test putty"); - //float Potmeter = potMeterIn.read(); MeasureTicker.attach(&MeasureTicker_act, 0.01f); bqc.add(&bq1).add(&bq2); - //BiQuadTicker.attach(&BiQuadTicker_act, 0.01f); //frequentie van 100 Hz - //TimeTracker.attach(&TimeTracker_act, 1.0f); QEI Encoder(D12, D13, NC, 32); // turns on encoder - //sampleT.attach(&sampleT_act, 0.1f); - //pc.printf("Reference velocity: %f rad/s \r\n", referenceVelocity); - //pc.printf("Potmeter: %f rad/s \r\n", Potmeter); while(1) { @@ -211,37 +170,11 @@ counts = Encoder.getPulses(); // gives position of encoder pc.printf("Resolution: %f pulses/rad \r\n",resolution); } - /* - // Encoder part - counts = Encoder.getPulses(); // gives position - DerivativeCounts = ((double) counts-countsPrev)/0.01; - - scope.set(0,counts); - scope.set(1,DerivativeCounts); - //scope.set(1,bqcDerivativeCounts); - scope.send(); - countsPrev = counts; - //pc.printf("Counts: %i rad/s \r\n", counts); - //pc.printf("Derivative Counts: %f rad/s \r\n", DerivativeCounts); - } - +/* if (BiQuadTicker_go){ BiQuadTicker_go=false; BiQuadFilter(); } - - if (FeedbackTicker_go){ - FeedbackTicker_go=false; - Feedback(); - - if (TimeTracker_go){ - TimeTracker_go=false; - TimeTrackerF(); - } - - if (sampleT_go){ - sampleT_go=false; - sample(); - }*/ +*/ } } \ No newline at end of file