First trial for Inverse Kinematics Feedforward implementation. No errors, not yet tested with board
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of prog_pract3_3_PI_controller by
main.cpp@0:43160ef59f9f, 2016-10-14 (annotated)
- Committer:
- GerhardBerman
- Date:
- Fri Oct 14 14:18:14 2016 +0000
- Revision:
- 0:43160ef59f9f
- Child:
- 1:ace33633653b
3 different tickers for P-controller, gives errors 'Variables undefined'
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 | 0:43160ef59f9f | 13 | AnalogIn potMeter1(A0); |
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 | 0:43160ef59f9f | 21 | Ticker MeasureTicker, BiQuadTicker, FeedbackTickere;// sampleT, TimeTracker; |
GerhardBerman | 0:43160ef59f9f | 22 | HIDScope scope(2); |
GerhardBerman | 0:43160ef59f9f | 23 | |
GerhardBerman | 0:43160ef59f9f | 24 | //set datatypes |
GerhardBerman | 0:43160ef59f9f | 25 | int counts; |
GerhardBerman | 0:43160ef59f9f | 26 | double DerivativeCounts; |
GerhardBerman | 0:43160ef59f9f | 27 | int countsPrev = 0; |
GerhardBerman | 0:43160ef59f9f | 28 | float referenceVelocity = 0; |
GerhardBerman | 0:43160ef59f9f | 29 | double bqcDerivativeCounts = 0; |
GerhardBerman | 0:43160ef59f9f | 30 | const double PI = 3.141592653589793; |
GerhardBerman | 0:43160ef59f9f | 31 | double Potmeter1 = potMeter1.read(); |
GerhardBerman | 0:43160ef59f9f | 32 | double Potmeter2 = potMeter2.read(); |
GerhardBerman | 0:43160ef59f9f | 33 | const int cw = 1; |
GerhardBerman | 0:43160ef59f9f | 34 | const int ccw = 0; |
GerhardBerman | 0:43160ef59f9f | 35 | |
GerhardBerman | 0:43160ef59f9f | 36 | //define encoder counts and degrees |
GerhardBerman | 0:43160ef59f9f | 37 | QEI Encoder(D12, D13, NC, 32); // turns on encoder |
GerhardBerman | 0:43160ef59f9f | 38 | int counts_per_revolution = 4200; |
GerhardBerman | 0:43160ef59f9f | 39 | const double gear_ratio = 131; |
GerhardBerman | 0:43160ef59f9f | 40 | const double resolution = counts_per_revolution/(2*PI/gear_ratio); //counts per radian |
GerhardBerman | 0:43160ef59f9f | 41 | |
GerhardBerman | 0:43160ef59f9f | 42 | //set BiQuad |
GerhardBerman | 0:43160ef59f9f | 43 | BiQuadChain bqc; |
GerhardBerman | 0:43160ef59f9f | 44 | BiQuad bq1(0.0186, 0.0743, 0.1114, 0.0743, 0.0186); //get numbers from butter filter MATLAB |
GerhardBerman | 0:43160ef59f9f | 45 | BiQuad bq2(1.0000, -1.5704, 1.2756, -0.4844, 0.0762); |
GerhardBerman | 0:43160ef59f9f | 46 | |
GerhardBerman | 0:43160ef59f9f | 47 | //set go-Ticker settings |
GerhardBerman | 0:43160ef59f9f | 48 | volatile bool MeasurePTicker_go=false, PTicker_go=false, MotorControllerTicker_go=false;// TimeTracker_go=false, sampleT_go=false; |
GerhardBerman | 0:43160ef59f9f | 49 | void MeasurePTicker_act(){MeasurePTicker_go=true;}; // Activates go-flags |
GerhardBerman | 0:43160ef59f9f | 50 | void PTicker_act(){PTicker_go=true;}; |
GerhardBerman | 0:43160ef59f9f | 51 | void MotorControllerTicker_act(){MotorControllerTicker_go=true;}; |
GerhardBerman | 0:43160ef59f9f | 52 | //void TimeTracker_act(){TimeTracker_go=true;}; |
GerhardBerman | 0:43160ef59f9f | 53 | //void sampleT_act(){sampleT_go=true;}; |
GerhardBerman | 0:43160ef59f9f | 54 | |
GerhardBerman | 0:43160ef59f9f | 55 | /* |
GerhardBerman | 0:43160ef59f9f | 56 | float GetReferenceVelocity() |
GerhardBerman | 0:43160ef59f9f | 57 | { |
GerhardBerman | 0:43160ef59f9f | 58 | // Returns reference velocity in rad/s. |
GerhardBerman | 0:43160ef59f9f | 59 | // Positive value means clockwise rotation. |
GerhardBerman | 0:43160ef59f9f | 60 | const float maxVelocity = 8.4; // in rad/s of course! |
GerhardBerman | 0:43160ef59f9f | 61 | if (button1 == 0){ |
GerhardBerman | 0:43160ef59f9f | 62 | led1=1; |
GerhardBerman | 0:43160ef59f9f | 63 | led2=0; |
GerhardBerman | 0:43160ef59f9f | 64 | // Counterclockwise rotation |
GerhardBerman | 0:43160ef59f9f | 65 | referenceVelocity = potMeterIn * maxVelocity; |
GerhardBerman | 0:43160ef59f9f | 66 | } |
GerhardBerman | 0:43160ef59f9f | 67 | else { |
GerhardBerman | 0:43160ef59f9f | 68 | led1=0; |
GerhardBerman | 0:43160ef59f9f | 69 | led2=1; |
GerhardBerman | 0:43160ef59f9f | 70 | // Clockwise rotation |
GerhardBerman | 0:43160ef59f9f | 71 | referenceVelocity = -1*potMeterIn * maxVelocity; |
GerhardBerman | 0:43160ef59f9f | 72 | } |
GerhardBerman | 0:43160ef59f9f | 73 | return referenceVelocity; |
GerhardBerman | 0:43160ef59f9f | 74 | } |
GerhardBerman | 0:43160ef59f9f | 75 | |
GerhardBerman | 0:43160ef59f9f | 76 | float FeedForwardControl(float referenceVelocity) |
GerhardBerman | 0:43160ef59f9f | 77 | { |
GerhardBerman | 0:43160ef59f9f | 78 | // very simple linear feed-forward control |
GerhardBerman | 0:43160ef59f9f | 79 | const float MotorGain=8.4; // unit: (rad/s) / PWM |
GerhardBerman | 0:43160ef59f9f | 80 | float motorValue = referenceVelocity / MotorGain; |
GerhardBerman | 0:43160ef59f9f | 81 | return motorValue; |
GerhardBerman | 0:43160ef59f9f | 82 | } |
GerhardBerman | 0:43160ef59f9f | 83 | |
GerhardBerman | 0:43160ef59f9f | 84 | void SetMotor1(float motorValue) |
GerhardBerman | 0:43160ef59f9f | 85 | { |
GerhardBerman | 0:43160ef59f9f | 86 | // Given -1<=motorValue<=1, this sets the PWM and direction |
GerhardBerman | 0:43160ef59f9f | 87 | // bits for motor 1. Positive value makes motor rotating |
GerhardBerman | 0:43160ef59f9f | 88 | // clockwise. motorValues outside range are truncated to |
GerhardBerman | 0:43160ef59f9f | 89 | // within range |
GerhardBerman | 0:43160ef59f9f | 90 | if (motorValue >=0) motor1DirectionPin=1; |
GerhardBerman | 0:43160ef59f9f | 91 | else motor1DirectionPin=0; |
GerhardBerman | 0:43160ef59f9f | 92 | if (fabs(motorValue)>1) motor1MagnitudePin = 1; |
GerhardBerman | 0:43160ef59f9f | 93 | else motor1MagnitudePin = fabs(motorValue); |
GerhardBerman | 0:43160ef59f9f | 94 | } |
GerhardBerman | 0:43160ef59f9f | 95 | |
GerhardBerman | 0:43160ef59f9f | 96 | void MeasureAndControl() |
GerhardBerman | 0:43160ef59f9f | 97 | { |
GerhardBerman | 0:43160ef59f9f | 98 | // This function measures the potmeter position, extracts a |
GerhardBerman | 0:43160ef59f9f | 99 | // reference velocity from it, and controls the motor with |
GerhardBerman | 0:43160ef59f9f | 100 | // a simple FeedForward controller. Call this from a Ticker. |
GerhardBerman | 0:43160ef59f9f | 101 | float referenceVelocity = GetReferenceVelocity(); |
GerhardBerman | 0:43160ef59f9f | 102 | float motorValue = FeedForwardControl(referenceVelocity); |
GerhardBerman | 0:43160ef59f9f | 103 | SetMotor1(motorValue); |
GerhardBerman | 0:43160ef59f9f | 104 | } |
GerhardBerman | 0:43160ef59f9f | 105 | |
GerhardBerman | 0:43160ef59f9f | 106 | void TimeTrackerF(){ |
GerhardBerman | 0:43160ef59f9f | 107 | wait(1); |
GerhardBerman | 0:43160ef59f9f | 108 | float Potmeter = potMeterIn.read(); |
GerhardBerman | 0:43160ef59f9f | 109 | pc.printf("Reference velocity: %f rad/s \r\n", referenceVelocity); |
GerhardBerman | 0:43160ef59f9f | 110 | pc.printf("Potmeter: %f rad/s \r\n", Potmeter); |
GerhardBerman | 0:43160ef59f9f | 111 | //pc.printf("Counts: %i rad/s \r\n", counts); |
GerhardBerman | 0:43160ef59f9f | 112 | //pc.printf("Derivative Counts: %i rad/s \r\n", DerivativeCounts); |
GerhardBerman | 0:43160ef59f9f | 113 | } |
GerhardBerman | 0:43160ef59f9f | 114 | |
GerhardBerman | 0:43160ef59f9f | 115 | void sample() |
GerhardBerman | 0:43160ef59f9f | 116 | { |
GerhardBerman | 0:43160ef59f9f | 117 | int countsPrev = 0; |
GerhardBerman | 0:43160ef59f9f | 118 | QEI Encoder(D12, D13, NC, 32); |
GerhardBerman | 0:43160ef59f9f | 119 | counts = Encoder.getPulses(); // gives position |
GerhardBerman | 0:43160ef59f9f | 120 | //scope.set(0,counts); |
GerhardBerman | 0:43160ef59f9f | 121 | DerivativeCounts = (counts-countsPrev)/0.001; |
GerhardBerman | 0:43160ef59f9f | 122 | //scope.set(1,DerivativeCounts); |
GerhardBerman | 0:43160ef59f9f | 123 | countsPrev = counts; |
GerhardBerman | 0:43160ef59f9f | 124 | //scope.send(); |
GerhardBerman | 0:43160ef59f9f | 125 | pc.printf("Counts: %i rad/s \r\n", counts); |
GerhardBerman | 0:43160ef59f9f | 126 | pc.printf("Derivative Counts: %d rad/s \r\n", DerivativeCounts); |
GerhardBerman | 0:43160ef59f9f | 127 | } |
GerhardBerman | 0:43160ef59f9f | 128 | |
GerhardBerman | 0:43160ef59f9f | 129 | void BiQuadFilter(){ //this function creates a BiQuad filter for the DerivativeCounts |
GerhardBerman | 0:43160ef59f9f | 130 | //double in=DerivativeCounts(); |
GerhardBerman | 0:43160ef59f9f | 131 | bqcDerivativeCounts=bqc.step(DerivativeCounts); |
GerhardBerman | 0:43160ef59f9f | 132 | //return(bqcDerivativeCounts); |
GerhardBerman | 0:43160ef59f9f | 133 | } |
GerhardBerman | 0:43160ef59f9f | 134 | */ |
GerhardBerman | 0:43160ef59f9f | 135 | void MeasureP(){ |
GerhardBerman | 0:43160ef59f9f | 136 | double ref_position = Potmeter1; //reference position from potmeter |
GerhardBerman | 0:43160ef59f9f | 137 | int counts = Encoder.getPulses(); // gives position |
GerhardBerman | 0:43160ef59f9f | 138 | double position = counts/resolution; //position in radians |
GerhardBerman | 0:43160ef59f9f | 139 | double rotation = ref_position-position; //rotation is 'position error' in radians |
GerhardBerman | 0:43160ef59f9f | 140 | double movement = rotation/(2*PI); //movement in rotations |
GerhardBerman | 0:43160ef59f9f | 141 | double Kp = Potmeter2; |
GerhardBerman | 0:43160ef59f9f | 142 | } |
GerhardBerman | 0:43160ef59f9f | 143 | |
GerhardBerman | 0:43160ef59f9f | 144 | double P(double rotation, double Kp){ |
GerhardBerman | 0:43160ef59f9f | 145 | double P_output = Kp*movement; |
GerhardBerman | 0:43160ef59f9f | 146 | return P_output; |
GerhardBerman | 0:43160ef59f9f | 147 | } |
GerhardBerman | 0:43160ef59f9f | 148 | |
GerhardBerman | 0:43160ef59f9f | 149 | void MotorController(){ |
GerhardBerman | 0:43160ef59f9f | 150 | double output = P(rotation, Kp); |
GerhardBerman | 0:43160ef59f9f | 151 | |
GerhardBerman | 0:43160ef59f9f | 152 | if(rotation>0){ |
GerhardBerman | 0:43160ef59f9f | 153 | motor1DirectionPin.write(cw); |
GerhardBerman | 0:43160ef59f9f | 154 | motor1MagnitudePin.write(output); |
GerhardBerman | 0:43160ef59f9f | 155 | } |
GerhardBerman | 0:43160ef59f9f | 156 | if(rotation<0){ |
GerhardBerman | 0:43160ef59f9f | 157 | motor1DirectionPin.write(ccw); |
GerhardBerman | 0:43160ef59f9f | 158 | motor1MagnitudePin.write(-output); |
GerhardBerman | 0:43160ef59f9f | 159 | } |
GerhardBerman | 0:43160ef59f9f | 160 | } |
GerhardBerman | 0:43160ef59f9f | 161 | |
GerhardBerman | 0:43160ef59f9f | 162 | int main() |
GerhardBerman | 0:43160ef59f9f | 163 | { |
GerhardBerman | 0:43160ef59f9f | 164 | //Initialize |
GerhardBerman | 0:43160ef59f9f | 165 | led1=0; |
GerhardBerman | 0:43160ef59f9f | 166 | led2=0; |
GerhardBerman | 0:43160ef59f9f | 167 | float Potmeter = potMeterIn.read(); |
GerhardBerman | 0:43160ef59f9f | 168 | MeasureP.attach(&MeasureP_act, 0.01f); |
GerhardBerman | 0:43160ef59f9f | 169 | P.attach(&P_act, 0.01f); |
GerhardBerman | 0:43160ef59f9f | 170 | MotorController.attach(&MotorController_act, 0.01f); |
GerhardBerman | 0:43160ef59f9f | 171 | pc.baud(115200); |
GerhardBerman | 0:43160ef59f9f | 172 | QEI Encoder(D12, D13, NC, 32); // turns on encoder |
GerhardBerman | 0:43160ef59f9f | 173 | //sampleT.attach(&sampleT_act, 0.1f); |
GerhardBerman | 0:43160ef59f9f | 174 | //pc.printf("Reference velocity: %f rad/s \r\n", referenceVelocity); |
GerhardBerman | 0:43160ef59f9f | 175 | //pc.printf("Potmeter: %f rad/s \r\n", Potmeter); |
GerhardBerman | 0:43160ef59f9f | 176 | |
GerhardBerman | 0:43160ef59f9f | 177 | while(1) |
GerhardBerman | 0:43160ef59f9f | 178 | { |
GerhardBerman | 0:43160ef59f9f | 179 | if (MeasureP_go){ |
GerhardBerman | 0:43160ef59f9f | 180 | MeasureP_go=false; |
GerhardBerman | 0:43160ef59f9f | 181 | MeasureP(); |
GerhardBerman | 0:43160ef59f9f | 182 | // Encoder part |
GerhardBerman | 0:43160ef59f9f | 183 | /* |
GerhardBerman | 0:43160ef59f9f | 184 | counts = Encoder.getPulses(); // gives position |
GerhardBerman | 0:43160ef59f9f | 185 | DerivativeCounts = ((double) counts-countsPrev)/0.01; |
GerhardBerman | 0:43160ef59f9f | 186 | |
GerhardBerman | 0:43160ef59f9f | 187 | scope.set(0,counts); |
GerhardBerman | 0:43160ef59f9f | 188 | scope.set(1,DerivativeCounts); |
GerhardBerman | 0:43160ef59f9f | 189 | //scope.set(1,bqcDerivativeCounts); |
GerhardBerman | 0:43160ef59f9f | 190 | scope.send(); |
GerhardBerman | 0:43160ef59f9f | 191 | countsPrev = counts; |
GerhardBerman | 0:43160ef59f9f | 192 | //pc.printf("Counts: %i rad/s \r\n", counts); |
GerhardBerman | 0:43160ef59f9f | 193 | //pc.printf("Derivative Counts: %f rad/s \r\n", DerivativeCounts); |
GerhardBerman | 0:43160ef59f9f | 194 | */ |
GerhardBerman | 0:43160ef59f9f | 195 | } |
GerhardBerman | 0:43160ef59f9f | 196 | |
GerhardBerman | 0:43160ef59f9f | 197 | if (PTicker_go){ |
GerhardBerman | 0:43160ef59f9f | 198 | PTicker_go=false; |
GerhardBerman | 0:43160ef59f9f | 199 | P(); |
GerhardBerman | 0:43160ef59f9f | 200 | } |
GerhardBerman | 0:43160ef59f9f | 201 | if (MotorControllerTicker_go){ |
GerhardBerman | 0:43160ef59f9f | 202 | MotorControllerTicker_go=false; |
GerhardBerman | 0:43160ef59f9f | 203 | MotorController(); |
GerhardBerman | 0:43160ef59f9f | 204 | /*if (TimeTracker_go){ |
GerhardBerman | 0:43160ef59f9f | 205 | TimeTracker_go=false; |
GerhardBerman | 0:43160ef59f9f | 206 | TimeTrackerF(); |
GerhardBerman | 0:43160ef59f9f | 207 | } |
GerhardBerman | 0:43160ef59f9f | 208 | if (sampleT_go){ |
GerhardBerman | 0:43160ef59f9f | 209 | sampleT_go=false; |
GerhardBerman | 0:43160ef59f9f | 210 | sample(); |
GerhardBerman | 0:43160ef59f9f | 211 | }*/ |
GerhardBerman | 0:43160ef59f9f | 212 | } |
GerhardBerman | 0:43160ef59f9f | 213 | } |