Version for EMG Threshold finding
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of prog_BioRobotics_Group9_StampRobot by
main.cpp@5:37e230689418, 2016-10-17 (annotated)
- Committer:
- GerhardBerman
- Date:
- Mon Oct 17 13:10:20 2016 +0000
- Revision:
- 5:37e230689418
- Parent:
- 4:19e376d31380
- Child:
- 6:3c4f3f2ce54f
PI Controller working with HIDScope
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
GerhardBerman | 0:43160ef59f9f | 1 | #include "mbed.h" |
GerhardBerman | 0:43160ef59f9f | 2 | #include <math.h> |
GerhardBerman | 0:43160ef59f9f | 3 | #include "MODSERIAL.h" |
GerhardBerman | 0:43160ef59f9f | 4 | #include "QEI.h" |
GerhardBerman | 0:43160ef59f9f | 5 | #include "HIDScope.h" |
GerhardBerman | 0:43160ef59f9f | 6 | #include "BiQuad.h" |
GerhardBerman | 0:43160ef59f9f | 7 | |
GerhardBerman | 0:43160ef59f9f | 8 | //set pins |
GerhardBerman | 0:43160ef59f9f | 9 | DigitalIn encoder1A (D13); //Channel A van Encoder 1 |
GerhardBerman | 0:43160ef59f9f | 10 | DigitalIn encoder1B (D12); //Channel B van Encoder 1 |
GerhardBerman | 0:43160ef59f9f | 11 | DigitalOut led1 (D11); |
GerhardBerman | 0:43160ef59f9f | 12 | DigitalOut led2 (D10); |
GerhardBerman | 3:8caef4872b0c | 13 | AnalogIn potMeter1(A2); |
GerhardBerman | 0:43160ef59f9f | 14 | AnalogIn potMeter2(A1); |
GerhardBerman | 0:43160ef59f9f | 15 | DigitalOut motor1DirectionPin(D7); |
GerhardBerman | 0:43160ef59f9f | 16 | PwmOut motor1MagnitudePin(D6); |
GerhardBerman | 0:43160ef59f9f | 17 | DigitalIn button1(D5); |
GerhardBerman | 0:43160ef59f9f | 18 | |
GerhardBerman | 0:43160ef59f9f | 19 | //set settings |
GerhardBerman | 0:43160ef59f9f | 20 | Serial pc(USBTX,USBRX); |
GerhardBerman | 3:8caef4872b0c | 21 | Ticker MeasureTicker, BiQuadTicker; //, TimeTracker; // sampleT; |
GerhardBerman | 5:37e230689418 | 22 | HIDScope scope(2); |
GerhardBerman | 0:43160ef59f9f | 23 | |
GerhardBerman | 0:43160ef59f9f | 24 | //set datatypes |
GerhardBerman | 3:8caef4872b0c | 25 | int counts = 0; |
GerhardBerman | 0:43160ef59f9f | 26 | double DerivativeCounts; |
GerhardBerman | 4:19e376d31380 | 27 | |
GerhardBerman | 4:19e376d31380 | 28 | float error_prev = 0; |
GerhardBerman | 4:19e376d31380 | 29 | float IntError = 0; |
GerhardBerman | 4:19e376d31380 | 30 | float t_sample = 0.01; //seconds |
GerhardBerman | 4:19e376d31380 | 31 | |
GerhardBerman | 0:43160ef59f9f | 32 | int countsPrev = 0; |
GerhardBerman | 0:43160ef59f9f | 33 | float referenceVelocity = 0; |
GerhardBerman | 3:8caef4872b0c | 34 | float bqcDerivativeCounts = 0; |
GerhardBerman | 3:8caef4872b0c | 35 | const float PI = 3.141592653589793; |
GerhardBerman | 3:8caef4872b0c | 36 | //float Potmeter1 = potMeter1.read(); |
GerhardBerman | 3:8caef4872b0c | 37 | //float Potmeter2 = potMeter2.read(); |
GerhardBerman | 3:8caef4872b0c | 38 | const int cw = 0; //values for cw and ccw are inverted!! cw=0 and ccw=1 |
GerhardBerman | 3:8caef4872b0c | 39 | const int ccw = 1; |
GerhardBerman | 0:43160ef59f9f | 40 | |
GerhardBerman | 0:43160ef59f9f | 41 | //set BiQuad |
GerhardBerman | 0:43160ef59f9f | 42 | BiQuadChain bqc; |
GerhardBerman | 0:43160ef59f9f | 43 | BiQuad bq1(0.0186, 0.0743, 0.1114, 0.0743, 0.0186); //get numbers from butter filter MATLAB |
GerhardBerman | 0:43160ef59f9f | 44 | BiQuad bq2(1.0000, -1.5704, 1.2756, -0.4844, 0.0762); |
GerhardBerman | 0:43160ef59f9f | 45 | |
GerhardBerman | 0:43160ef59f9f | 46 | //set go-Ticker settings |
GerhardBerman | 3:8caef4872b0c | 47 | volatile bool MeasureTicker_go=false, BiQuadTicker_go=false, FeedbackTicker_go=false, TimeTracker_go=false; // sampleT_go=false; |
GerhardBerman | 3:8caef4872b0c | 48 | void MeasureTicker_act(){MeasureTicker_go=true;}; // Activates go-flags |
GerhardBerman | 3:8caef4872b0c | 49 | void BiQuadTicker_act(){BiQuadTicker_go=true;}; |
GerhardBerman | 3:8caef4872b0c | 50 | void FeedbackTicker_act(){FeedbackTicker_go=true;}; |
GerhardBerman | 3:8caef4872b0c | 51 | void TimeTracker_act(){TimeTracker_go=true;}; |
GerhardBerman | 3:8caef4872b0c | 52 | //void sampleT_act(){sampleT_go=true;}; |
GerhardBerman | 3:8caef4872b0c | 53 | |
GerhardBerman | 3:8caef4872b0c | 54 | //define encoder counts and degrees |
GerhardBerman | 3:8caef4872b0c | 55 | QEI Encoder(D12, D13, NC, 32); // turns on encoder |
GerhardBerman | 4:19e376d31380 | 56 | const int counts_per_revolution = 4200; //counts per motor axis revolution |
GerhardBerman | 4:19e376d31380 | 57 | const int inverse_gear_ratio = 131; |
GerhardBerman | 4:19e376d31380 | 58 | //const float motor_axial_resolution = counts_per_revolution/(2*PI); |
GerhardBerman | 4:19e376d31380 | 59 | const float resolution = counts_per_revolution/(2*PI/inverse_gear_ratio); //87567.0496892 counts per radian, encoder axis |
GerhardBerman | 3:8caef4872b0c | 60 | |
GerhardBerman | 3:8caef4872b0c | 61 | float GetReferencePosition() |
GerhardBerman | 3:8caef4872b0c | 62 | { |
GerhardBerman | 3:8caef4872b0c | 63 | // Returns reference position in rad. |
GerhardBerman | 3:8caef4872b0c | 64 | // Positive value means clockwise rotation. |
GerhardBerman | 3:8caef4872b0c | 65 | const float maxPosition = 2*PI; //6.283185307179586; // in radians |
GerhardBerman | 3:8caef4872b0c | 66 | float Potmeter1 = potMeter1.read(); |
GerhardBerman | 3:8caef4872b0c | 67 | float referencePosition = Potmeter1 * maxPosition; //Potmeter1 * maxPosition; //refpos in radians |
GerhardBerman | 3:8caef4872b0c | 68 | pc.printf("Max Position: %f rad \r\n", maxPosition); |
GerhardBerman | 4:19e376d31380 | 69 | pc.printf("Potmeter1, refpos: %f \r\n", Potmeter1); |
GerhardBerman | 4:19e376d31380 | 70 | pc.printf("Motor Axis Ref Position: %f rad \r\n", referencePosition); |
GerhardBerman | 3:8caef4872b0c | 71 | return referencePosition; |
GerhardBerman | 3:8caef4872b0c | 72 | } |
GerhardBerman | 3:8caef4872b0c | 73 | |
GerhardBerman | 3:8caef4872b0c | 74 | float FeedForwardControl(float referencePosition) |
GerhardBerman | 3:8caef4872b0c | 75 | { |
GerhardBerman | 4:19e376d31380 | 76 | float EncoderPosition = counts/resolution; //position in radians, encoder axis |
GerhardBerman | 4:19e376d31380 | 77 | float Position = EncoderPosition*inverse_gear_ratio; //position in radians, motor axis |
GerhardBerman | 3:8caef4872b0c | 78 | // linear feedback control |
GerhardBerman | 5:37e230689418 | 79 | scope.set(0,referencePosition); |
GerhardBerman | 5:37e230689418 | 80 | scope.set(1,Position); |
GerhardBerman | 5:37e230689418 | 81 | scope.send(); |
GerhardBerman | 4:19e376d31380 | 82 | float error = referencePosition - Position; // 'error' in radians |
GerhardBerman | 4:19e376d31380 | 83 | float Kp = 1; //potMeter2.read(); |
GerhardBerman | 4:19e376d31380 | 84 | |
GerhardBerman | 4:19e376d31380 | 85 | float IntError = IntError + error*t_sample; |
GerhardBerman | 5:37e230689418 | 86 | float maxKi = 0.2; |
GerhardBerman | 5:37e230689418 | 87 | float Ki = potMeter2.read()*maxKi; |
GerhardBerman | 4:19e376d31380 | 88 | |
GerhardBerman | 4:19e376d31380 | 89 | //float DerivativeError = (error_prev + error)/t_sample; |
GerhardBerman | 5:37e230689418 | 90 | float maxKd = 0.2; |
GerhardBerman | 5:37e230689418 | 91 | //float Kd = potMeter2.read()*maxKd; |
GerhardBerman | 4:19e376d31380 | 92 | |
GerhardBerman | 4:19e376d31380 | 93 | float motorValue = error * Kp + IntError * Ki; //+ DerivativeError * Kd; |
GerhardBerman | 4:19e376d31380 | 94 | pc.printf("Motor Axis Position: %f rad \r\n", Position); |
GerhardBerman | 4:19e376d31380 | 95 | pc.printf("Counts encoder: %i rad \r\n", counts); |
GerhardBerman | 3:8caef4872b0c | 96 | pc.printf("Kp: %f \r\n", Kp); |
GerhardBerman | 3:8caef4872b0c | 97 | pc.printf("MotorValue: %f \r\n", motorValue); |
GerhardBerman | 4:19e376d31380 | 98 | |
GerhardBerman | 4:19e376d31380 | 99 | error_prev = error; |
GerhardBerman | 3:8caef4872b0c | 100 | return motorValue; |
GerhardBerman | 3:8caef4872b0c | 101 | } |
GerhardBerman | 0:43160ef59f9f | 102 | |
GerhardBerman | 3:8caef4872b0c | 103 | void SetMotor1(float motorValue) |
GerhardBerman | 3:8caef4872b0c | 104 | { |
GerhardBerman | 3:8caef4872b0c | 105 | // Given -1<=motorValue<=1, this sets the PWM and direction |
GerhardBerman | 3:8caef4872b0c | 106 | // bits for motor 1. Positive value makes motor rotating |
GerhardBerman | 3:8caef4872b0c | 107 | // clockwise. motorValues outside range are truncated to |
GerhardBerman | 3:8caef4872b0c | 108 | // within range |
GerhardBerman | 3:8caef4872b0c | 109 | if (motorValue >=0) |
GerhardBerman | 3:8caef4872b0c | 110 | {motor1DirectionPin=cw; |
GerhardBerman | 3:8caef4872b0c | 111 | led1=1; |
GerhardBerman | 3:8caef4872b0c | 112 | led2=0; |
GerhardBerman | 3:8caef4872b0c | 113 | } |
GerhardBerman | 3:8caef4872b0c | 114 | else {motor1DirectionPin=ccw; |
GerhardBerman | 3:8caef4872b0c | 115 | led1=0; |
GerhardBerman | 3:8caef4872b0c | 116 | led2=1; |
GerhardBerman | 3:8caef4872b0c | 117 | } |
GerhardBerman | 3:8caef4872b0c | 118 | if (fabs(motorValue)>1) motor1MagnitudePin = 1; |
GerhardBerman | 3:8caef4872b0c | 119 | else motor1MagnitudePin = fabs(motorValue); |
GerhardBerman | 3:8caef4872b0c | 120 | } |
GerhardBerman | 3:8caef4872b0c | 121 | |
GerhardBerman | 3:8caef4872b0c | 122 | void MeasureAndControl() |
GerhardBerman | 3:8caef4872b0c | 123 | { |
GerhardBerman | 3:8caef4872b0c | 124 | // This function measures the potmeter position, extracts a |
GerhardBerman | 3:8caef4872b0c | 125 | // reference position from it, and controls the motor with |
GerhardBerman | 3:8caef4872b0c | 126 | // a Feedback controller. Call this from a Ticker. |
GerhardBerman | 3:8caef4872b0c | 127 | float referencePosition = GetReferencePosition(); |
GerhardBerman | 3:8caef4872b0c | 128 | float motorValue = FeedForwardControl(referencePosition); |
GerhardBerman | 3:8caef4872b0c | 129 | SetMotor1(motorValue); |
GerhardBerman | 3:8caef4872b0c | 130 | } |
GerhardBerman | 3:8caef4872b0c | 131 | |
GerhardBerman | 3:8caef4872b0c | 132 | void TimeTrackerF(){ |
GerhardBerman | 3:8caef4872b0c | 133 | //wait(1); |
GerhardBerman | 3:8caef4872b0c | 134 | //float Potmeter1 = potMeter1.read(); |
GerhardBerman | 3:8caef4872b0c | 135 | float referencePosition = GetReferencePosition(); |
GerhardBerman | 3:8caef4872b0c | 136 | pc.printf("TTReference Position: %d rad \r\n", referencePosition); |
GerhardBerman | 3:8caef4872b0c | 137 | //pc.printf("TTPotmeter1, for refpos: %f \r\n", Potmeter1); |
GerhardBerman | 3:8caef4872b0c | 138 | //pc.printf("TTPotmeter2, Kp: %f \r\n", Potmeter2); |
GerhardBerman | 3:8caef4872b0c | 139 | pc.printf("TTCounts: %i \r\n", counts); |
GerhardBerman | 3:8caef4872b0c | 140 | } |
GerhardBerman | 3:8caef4872b0c | 141 | /* |
GerhardBerman | 3:8caef4872b0c | 142 | void sample() |
GerhardBerman | 3:8caef4872b0c | 143 | { |
GerhardBerman | 3:8caef4872b0c | 144 | int countsPrev = 0; |
GerhardBerman | 3:8caef4872b0c | 145 | QEI Encoder(D12, D13, NC, 32); |
GerhardBerman | 3:8caef4872b0c | 146 | counts = Encoder.getPulses(); // gives position |
GerhardBerman | 3:8caef4872b0c | 147 | //scope.set(0,counts); |
GerhardBerman | 3:8caef4872b0c | 148 | DerivativeCounts = (counts-countsPrev)/0.001; |
GerhardBerman | 3:8caef4872b0c | 149 | //scope.set(1,DerivativeCounts); |
GerhardBerman | 3:8caef4872b0c | 150 | countsPrev = counts; |
GerhardBerman | 3:8caef4872b0c | 151 | //scope.send(); |
GerhardBerman | 3:8caef4872b0c | 152 | pc.printf("Counts: %i rad/s \r\n", counts); |
GerhardBerman | 3:8caef4872b0c | 153 | pc.printf("Derivative Counts: %d rad/s \r\n", DerivativeCounts); |
GerhardBerman | 3:8caef4872b0c | 154 | } |
GerhardBerman | 3:8caef4872b0c | 155 | |
GerhardBerman | 3:8caef4872b0c | 156 | void BiQuadFilter(){ //this function creates a BiQuad filter for the DerivativeCounts |
GerhardBerman | 3:8caef4872b0c | 157 | //double in=DerivativeCounts(); |
GerhardBerman | 3:8caef4872b0c | 158 | bqcDerivativeCounts=bqc.step(DerivativeCounts); |
GerhardBerman | 3:8caef4872b0c | 159 | //return(bqcDerivativeCounts); |
GerhardBerman | 3:8caef4872b0c | 160 | } |
GerhardBerman | 3:8caef4872b0c | 161 | |
GerhardBerman | 0:43160ef59f9f | 162 | void MeasureP(){ |
GerhardBerman | 0:43160ef59f9f | 163 | double ref_position = Potmeter1; //reference position from potmeter |
GerhardBerman | 0:43160ef59f9f | 164 | int counts = Encoder.getPulses(); // gives position |
GerhardBerman | 0:43160ef59f9f | 165 | double position = counts/resolution; //position in radians |
GerhardBerman | 0:43160ef59f9f | 166 | double rotation = ref_position-position; //rotation is 'position error' in radians |
GerhardBerman | 0:43160ef59f9f | 167 | double movement = rotation/(2*PI); //movement in rotations |
GerhardBerman | 0:43160ef59f9f | 168 | double Kp = Potmeter2; |
GerhardBerman | 3:8caef4872b0c | 169 | } |
GerhardBerman | 3:8caef4872b0c | 170 | |
GerhardBerman | 3:8caef4872b0c | 171 | double P(double rotation, double Kp){ |
GerhardBerman | 0:43160ef59f9f | 172 | double P_output = Kp*movement; |
GerhardBerman | 3:8caef4872b0c | 173 | return P_output; |
GerhardBerman | 3:8caef4872b0c | 174 | } |
GerhardBerman | 3:8caef4872b0c | 175 | |
GerhardBerman | 3:8caef4872b0c | 176 | void MotorController(){ |
GerhardBerman | 3:8caef4872b0c | 177 | double output = P(rotation, Kp); |
GerhardBerman | 0:43160ef59f9f | 178 | |
GerhardBerman | 0:43160ef59f9f | 179 | if(rotation>0){ |
GerhardBerman | 0:43160ef59f9f | 180 | motor1DirectionPin.write(cw); |
GerhardBerman | 3:8caef4872b0c | 181 | motor1MagnitudePin.write(output); |
GerhardBerman | 0:43160ef59f9f | 182 | } |
GerhardBerman | 0:43160ef59f9f | 183 | if(rotation<0){ |
GerhardBerman | 0:43160ef59f9f | 184 | motor1DirectionPin.write(ccw); |
GerhardBerman | 3:8caef4872b0c | 185 | motor1MagnitudePin.write(-output); |
GerhardBerman | 0:43160ef59f9f | 186 | } |
GerhardBerman | 0:43160ef59f9f | 187 | } |
GerhardBerman | 3:8caef4872b0c | 188 | */ |
GerhardBerman | 0:43160ef59f9f | 189 | int main() |
GerhardBerman | 0:43160ef59f9f | 190 | { |
GerhardBerman | 0:43160ef59f9f | 191 | //Initialize |
GerhardBerman | 3:8caef4872b0c | 192 | led1=1; |
GerhardBerman | 3:8caef4872b0c | 193 | led2=1; |
GerhardBerman | 3:8caef4872b0c | 194 | pc.baud(115200); |
GerhardBerman | 3:8caef4872b0c | 195 | pc.printf("Test putty"); |
GerhardBerman | 3:8caef4872b0c | 196 | //float Potmeter = potMeterIn.read(); |
GerhardBerman | 4:19e376d31380 | 197 | MeasureTicker.attach(&MeasureTicker_act, 0.01f); |
GerhardBerman | 3:8caef4872b0c | 198 | bqc.add(&bq1).add(&bq2); |
GerhardBerman | 3:8caef4872b0c | 199 | //BiQuadTicker.attach(&BiQuadTicker_act, 0.01f); //frequentie van 100 Hz |
GerhardBerman | 3:8caef4872b0c | 200 | //TimeTracker.attach(&TimeTracker_act, 1.0f); |
GerhardBerman | 0:43160ef59f9f | 201 | QEI Encoder(D12, D13, NC, 32); // turns on encoder |
GerhardBerman | 3:8caef4872b0c | 202 | //sampleT.attach(&sampleT_act, 0.1f); |
GerhardBerman | 3:8caef4872b0c | 203 | //pc.printf("Reference velocity: %f rad/s \r\n", referenceVelocity); |
GerhardBerman | 3:8caef4872b0c | 204 | //pc.printf("Potmeter: %f rad/s \r\n", Potmeter); |
GerhardBerman | 0:43160ef59f9f | 205 | |
GerhardBerman | 0:43160ef59f9f | 206 | while(1) |
GerhardBerman | 0:43160ef59f9f | 207 | { |
GerhardBerman | 3:8caef4872b0c | 208 | if (MeasureTicker_go){ |
GerhardBerman | 3:8caef4872b0c | 209 | MeasureTicker_go=false; |
GerhardBerman | 3:8caef4872b0c | 210 | MeasureAndControl(); |
GerhardBerman | 3:8caef4872b0c | 211 | counts = Encoder.getPulses(); // gives position of encoder |
GerhardBerman | 3:8caef4872b0c | 212 | pc.printf("Resolution: %f pulses/rad \r\n",resolution); |
GerhardBerman | 3:8caef4872b0c | 213 | } |
GerhardBerman | 3:8caef4872b0c | 214 | /* |
GerhardBerman | 3:8caef4872b0c | 215 | // Encoder part |
GerhardBerman | 3:8caef4872b0c | 216 | counts = Encoder.getPulses(); // gives position |
GerhardBerman | 3:8caef4872b0c | 217 | DerivativeCounts = ((double) counts-countsPrev)/0.01; |
GerhardBerman | 3:8caef4872b0c | 218 | |
GerhardBerman | 3:8caef4872b0c | 219 | scope.set(0,counts); |
GerhardBerman | 3:8caef4872b0c | 220 | scope.set(1,DerivativeCounts); |
GerhardBerman | 3:8caef4872b0c | 221 | //scope.set(1,bqcDerivativeCounts); |
GerhardBerman | 3:8caef4872b0c | 222 | scope.send(); |
GerhardBerman | 3:8caef4872b0c | 223 | countsPrev = counts; |
GerhardBerman | 3:8caef4872b0c | 224 | //pc.printf("Counts: %i rad/s \r\n", counts); |
GerhardBerman | 3:8caef4872b0c | 225 | //pc.printf("Derivative Counts: %f rad/s \r\n", DerivativeCounts); |
GerhardBerman | 1:ace33633653b | 226 | } |
GerhardBerman | 3:8caef4872b0c | 227 | |
GerhardBerman | 3:8caef4872b0c | 228 | if (BiQuadTicker_go){ |
GerhardBerman | 3:8caef4872b0c | 229 | BiQuadTicker_go=false; |
GerhardBerman | 3:8caef4872b0c | 230 | BiQuadFilter(); |
GerhardBerman | 3:8caef4872b0c | 231 | } |
GerhardBerman | 3:8caef4872b0c | 232 | |
GerhardBerman | 3:8caef4872b0c | 233 | if (FeedbackTicker_go){ |
GerhardBerman | 3:8caef4872b0c | 234 | FeedbackTicker_go=false; |
GerhardBerman | 3:8caef4872b0c | 235 | Feedback(); |
GerhardBerman | 3:8caef4872b0c | 236 | |
GerhardBerman | 3:8caef4872b0c | 237 | if (TimeTracker_go){ |
GerhardBerman | 3:8caef4872b0c | 238 | TimeTracker_go=false; |
GerhardBerman | 3:8caef4872b0c | 239 | TimeTrackerF(); |
GerhardBerman | 3:8caef4872b0c | 240 | } |
GerhardBerman | 3:8caef4872b0c | 241 | |
GerhardBerman | 3:8caef4872b0c | 242 | if (sampleT_go){ |
GerhardBerman | 3:8caef4872b0c | 243 | sampleT_go=false; |
GerhardBerman | 3:8caef4872b0c | 244 | sample(); |
GerhardBerman | 3:8caef4872b0c | 245 | }*/ |
GerhardBerman | 0:43160ef59f9f | 246 | } |
GerhardBerman | 0:43160ef59f9f | 247 | } |