All working, HIDScope working with BiQuads! Filter coefficients are not perfect yet.
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of prog_practPutty2 by
main.cpp@5:0bc7a874c640, 2016-10-17 (annotated)
- Committer:
- GerhardBerman
- Date:
- Mon Oct 17 10:38:56 2016 +0000
- Revision:
- 5:0bc7a874c640
- Parent:
- 4:7224b1f0c668
All working, including BiQuads and HIDScope. Filter coeffs are not perfect yet.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Marieke | 0:818fc79663f2 | 1 | #include "mbed.h" |
Marieke | 0:818fc79663f2 | 2 | #include <math.h> |
Marieke | 0:818fc79663f2 | 3 | #include "MODSERIAL.h" |
Marieke | 0:818fc79663f2 | 4 | #include "QEI.h" |
Marieke | 2:915abd97ac16 | 5 | #include "HIDScope.h" |
GerhardBerman | 4:7224b1f0c668 | 6 | #include "BiQuad.h" |
Marieke | 0:818fc79663f2 | 7 | |
Marieke | 0:818fc79663f2 | 8 | DigitalIn encoder1A (D13); //Channel A van Encoder 1 |
Marieke | 0:818fc79663f2 | 9 | DigitalIn encoder1B (D12); //Channel B van Encoder 1 |
Marieke | 0:818fc79663f2 | 10 | DigitalOut led1 (D11); |
Marieke | 0:818fc79663f2 | 11 | DigitalOut led2 (D10); |
Marieke | 0:818fc79663f2 | 12 | AnalogIn potMeterIn(A0); |
Marieke | 0:818fc79663f2 | 13 | DigitalOut motor1DirectionPin(D7); |
Marieke | 0:818fc79663f2 | 14 | PwmOut motor1MagnitudePin(D6); |
Marieke | 0:818fc79663f2 | 15 | DigitalIn button1(D5); |
Marieke | 0:818fc79663f2 | 16 | |
Marieke | 0:818fc79663f2 | 17 | |
Marieke | 0:818fc79663f2 | 18 | Serial pc(USBTX,USBRX); |
GerhardBerman | 4:7224b1f0c668 | 19 | Ticker MeasureTicker, BiQuadTicker;// sampleT, TimeTracker; |
Marieke | 2:915abd97ac16 | 20 | HIDScope scope(2); |
Marieke | 0:818fc79663f2 | 21 | int counts; |
Marieke | 2:915abd97ac16 | 22 | double DerivativeCounts; |
Marieke | 0:818fc79663f2 | 23 | int countsPrev = 0; |
Marieke | 0:818fc79663f2 | 24 | |
Marieke | 0:818fc79663f2 | 25 | float referenceVelocity = 0; |
GerhardBerman | 4:7224b1f0c668 | 26 | double bqcDerivativeCounts = 0; |
Marieke | 0:818fc79663f2 | 27 | |
GerhardBerman | 4:7224b1f0c668 | 28 | BiQuadChain bqc; |
GerhardBerman | 4:7224b1f0c668 | 29 | BiQuad bq1(0.0048,0.0193,0.0289,0.0193,0.0048); //get numbers from butter filter MATLAB |
GerhardBerman | 4:7224b1f0c668 | 30 | BiQuad bq2(1.0000,-2.3695,2.3140,-1.0547,0.1874); |
GerhardBerman | 4:7224b1f0c668 | 31 | |
GerhardBerman | 4:7224b1f0c668 | 32 | volatile bool MeasureTicker_go=false, BiQuadTicker_go=false;// TimeTracker_go=false, sampleT_go=false; |
Marieke | 0:818fc79663f2 | 33 | |
Marieke | 0:818fc79663f2 | 34 | void MeasureTicker_act(){MeasureTicker_go=true;}; // Activates go-flags |
GerhardBerman | 4:7224b1f0c668 | 35 | void BiQuadTicker_act(){BiQuadTicker_go=true;}; |
Marieke | 2:915abd97ac16 | 36 | //void TimeTracker_act(){TimeTracker_go=true;}; |
Marieke | 2:915abd97ac16 | 37 | //void sampleT_act(){sampleT_go=true;}; |
Marieke | 0:818fc79663f2 | 38 | |
Marieke | 0:818fc79663f2 | 39 | float GetReferenceVelocity() |
Marieke | 0:818fc79663f2 | 40 | { |
Marieke | 0:818fc79663f2 | 41 | // Returns reference velocity in rad/s. |
Marieke | 0:818fc79663f2 | 42 | // Positive value means clockwise rotation. |
Marieke | 0:818fc79663f2 | 43 | const float maxVelocity = 8.4; // in rad/s of course! |
Marieke | 0:818fc79663f2 | 44 | if (button1 == 0){ |
Marieke | 0:818fc79663f2 | 45 | led1=1; |
Marieke | 0:818fc79663f2 | 46 | led2=0; |
Marieke | 0:818fc79663f2 | 47 | // Counterclockwise rotation |
Marieke | 0:818fc79663f2 | 48 | referenceVelocity = potMeterIn * maxVelocity; |
Marieke | 0:818fc79663f2 | 49 | } |
Marieke | 0:818fc79663f2 | 50 | else { |
Marieke | 0:818fc79663f2 | 51 | led1=0; |
Marieke | 0:818fc79663f2 | 52 | led2=1; |
Marieke | 0:818fc79663f2 | 53 | // Clockwise rotation |
Marieke | 0:818fc79663f2 | 54 | referenceVelocity = -1*potMeterIn * maxVelocity; |
Marieke | 0:818fc79663f2 | 55 | } |
Marieke | 0:818fc79663f2 | 56 | return referenceVelocity; |
Marieke | 0:818fc79663f2 | 57 | } |
Marieke | 0:818fc79663f2 | 58 | |
Marieke | 0:818fc79663f2 | 59 | float FeedForwardControl(float referenceVelocity) |
Marieke | 0:818fc79663f2 | 60 | { |
Marieke | 0:818fc79663f2 | 61 | // very simple linear feed-forward control |
Marieke | 0:818fc79663f2 | 62 | const float MotorGain=8.4; // unit: (rad/s) / PWM |
Marieke | 0:818fc79663f2 | 63 | float motorValue = referenceVelocity / MotorGain; |
Marieke | 0:818fc79663f2 | 64 | return motorValue; |
Marieke | 0:818fc79663f2 | 65 | } |
Marieke | 0:818fc79663f2 | 66 | |
Marieke | 0:818fc79663f2 | 67 | void SetMotor1(float motorValue) |
Marieke | 0:818fc79663f2 | 68 | { |
Marieke | 0:818fc79663f2 | 69 | // Given -1<=motorValue<=1, this sets the PWM and direction |
Marieke | 0:818fc79663f2 | 70 | // bits for motor 1. Positive value makes motor rotating |
Marieke | 0:818fc79663f2 | 71 | // clockwise. motorValues outside range are truncated to |
Marieke | 0:818fc79663f2 | 72 | // within range |
Marieke | 0:818fc79663f2 | 73 | if (motorValue >=0) motor1DirectionPin=1; |
Marieke | 0:818fc79663f2 | 74 | else motor1DirectionPin=0; |
Marieke | 0:818fc79663f2 | 75 | if (fabs(motorValue)>1) motor1MagnitudePin = 1; |
Marieke | 0:818fc79663f2 | 76 | else motor1MagnitudePin = fabs(motorValue); |
Marieke | 0:818fc79663f2 | 77 | } |
Marieke | 0:818fc79663f2 | 78 | |
Marieke | 0:818fc79663f2 | 79 | void MeasureAndControl() |
Marieke | 0:818fc79663f2 | 80 | { |
Marieke | 0:818fc79663f2 | 81 | // This function measures the potmeter position, extracts a |
Marieke | 0:818fc79663f2 | 82 | // reference velocity from it, and controls the motor with |
Marieke | 0:818fc79663f2 | 83 | // a simple FeedForward controller. Call this from a Ticker. |
Marieke | 0:818fc79663f2 | 84 | float referenceVelocity = GetReferenceVelocity(); |
Marieke | 0:818fc79663f2 | 85 | float motorValue = FeedForwardControl(referenceVelocity); |
Marieke | 0:818fc79663f2 | 86 | SetMotor1(motorValue); |
Marieke | 0:818fc79663f2 | 87 | } |
Marieke | 0:818fc79663f2 | 88 | |
Marieke | 0:818fc79663f2 | 89 | void TimeTrackerF(){ |
Marieke | 0:818fc79663f2 | 90 | wait(1); |
Marieke | 0:818fc79663f2 | 91 | float Potmeter = potMeterIn.read(); |
Marieke | 0:818fc79663f2 | 92 | pc.printf("Reference velocity: %f rad/s \r\n", referenceVelocity); |
Marieke | 0:818fc79663f2 | 93 | pc.printf("Potmeter: %f rad/s \r\n", Potmeter); |
Marieke | 0:818fc79663f2 | 94 | //pc.printf("Counts: %i rad/s \r\n", counts); |
Marieke | 0:818fc79663f2 | 95 | //pc.printf("Derivative Counts: %i rad/s \r\n", DerivativeCounts); |
Marieke | 0:818fc79663f2 | 96 | } |
Marieke | 0:818fc79663f2 | 97 | /* |
Marieke | 0:818fc79663f2 | 98 | void sample() |
Marieke | 0:818fc79663f2 | 99 | { |
Marieke | 0:818fc79663f2 | 100 | int countsPrev = 0; |
Marieke | 0:818fc79663f2 | 101 | QEI Encoder(D12, D13, NC, 32); |
Marieke | 0:818fc79663f2 | 102 | counts = Encoder.getPulses(); // gives position |
Marieke | 0:818fc79663f2 | 103 | //scope.set(0,counts); |
Marieke | 0:818fc79663f2 | 104 | DerivativeCounts = (counts-countsPrev)/0.001; |
Marieke | 0:818fc79663f2 | 105 | //scope.set(1,DerivativeCounts); |
Marieke | 0:818fc79663f2 | 106 | countsPrev = counts; |
Marieke | 0:818fc79663f2 | 107 | //scope.send(); |
Marieke | 0:818fc79663f2 | 108 | pc.printf("Counts: %i rad/s \r\n", counts); |
Marieke | 0:818fc79663f2 | 109 | pc.printf("Derivative Counts: %d rad/s \r\n", DerivativeCounts); |
Marieke | 0:818fc79663f2 | 110 | }*/ |
Marieke | 0:818fc79663f2 | 111 | |
GerhardBerman | 4:7224b1f0c668 | 112 | void BiQuadFilter(){ |
GerhardBerman | 4:7224b1f0c668 | 113 | //double in=DerivativeCounts(); |
GerhardBerman | 4:7224b1f0c668 | 114 | bqcDerivativeCounts=bqc.step(DerivativeCounts); |
GerhardBerman | 4:7224b1f0c668 | 115 | //return(bqcDerivativeCounts); |
GerhardBerman | 4:7224b1f0c668 | 116 | } |
GerhardBerman | 4:7224b1f0c668 | 117 | |
Marieke | 0:818fc79663f2 | 118 | int main() |
Marieke | 0:818fc79663f2 | 119 | { |
Marieke | 0:818fc79663f2 | 120 | //Initialize |
GerhardBerman | 5:0bc7a874c640 | 121 | led1=1; |
GerhardBerman | 5:0bc7a874c640 | 122 | led2=1; |
Marieke | 0:818fc79663f2 | 123 | float Potmeter = potMeterIn.read(); |
Marieke | 0:818fc79663f2 | 124 | MeasureTicker.attach(&MeasureTicker_act, 0.01f); |
GerhardBerman | 4:7224b1f0c668 | 125 | bqc.add(&bq1).add(&bq2); |
GerhardBerman | 4:7224b1f0c668 | 126 | BiQuadTicker.attach(&BiQuadTicker_act, 0.01f); //frequentie van 100 HZ |
Marieke | 2:915abd97ac16 | 127 | //TimeTracker.attach(&TimeTracker_act, 0.1f); |
Marieke | 0:818fc79663f2 | 128 | pc.baud(115200); |
Marieke | 0:818fc79663f2 | 129 | QEI Encoder(D12, D13, NC, 32); // turns on encoder |
Marieke | 0:818fc79663f2 | 130 | //sampleT.attach(&sampleT_act, 0.1f); |
Marieke | 2:915abd97ac16 | 131 | //pc.printf("Reference velocity: %f rad/s \r\n", referenceVelocity); |
Marieke | 2:915abd97ac16 | 132 | //pc.printf("Potmeter: %f rad/s \r\n", Potmeter); |
Marieke | 0:818fc79663f2 | 133 | |
Marieke | 0:818fc79663f2 | 134 | while(1) |
Marieke | 0:818fc79663f2 | 135 | { |
Marieke | 0:818fc79663f2 | 136 | if (MeasureTicker_go){ |
Marieke | 0:818fc79663f2 | 137 | MeasureTicker_go=false; |
Marieke | 0:818fc79663f2 | 138 | MeasureAndControl(); |
Marieke | 0:818fc79663f2 | 139 | // Encoder part |
Marieke | 0:818fc79663f2 | 140 | counts = Encoder.getPulses(); // gives position |
GerhardBerman | 4:7224b1f0c668 | 141 | DerivativeCounts = ((double) counts-countsPrev)/0.01; |
GerhardBerman | 4:7224b1f0c668 | 142 | |
Marieke | 1:812a1637b6cb | 143 | scope.set(0,counts); |
GerhardBerman | 4:7224b1f0c668 | 144 | //scope.set(1,DerivativeCounts); |
GerhardBerman | 4:7224b1f0c668 | 145 | scope.set(1,bqcDerivativeCounts); |
GerhardBerman | 4:7224b1f0c668 | 146 | scope.send(); |
GerhardBerman | 4:7224b1f0c668 | 147 | countsPrev = counts; |
Marieke | 2:915abd97ac16 | 148 | //pc.printf("Counts: %i rad/s \r\n", counts); |
Marieke | 2:915abd97ac16 | 149 | //pc.printf("Derivative Counts: %f rad/s \r\n", DerivativeCounts); |
Marieke | 0:818fc79663f2 | 150 | } |
GerhardBerman | 4:7224b1f0c668 | 151 | if (BiQuadTicker_go){ |
GerhardBerman | 4:7224b1f0c668 | 152 | BiQuadTicker_go=false; |
GerhardBerman | 4:7224b1f0c668 | 153 | BiQuadFilter(); |
GerhardBerman | 4:7224b1f0c668 | 154 | } |
Marieke | 2:915abd97ac16 | 155 | /*if (TimeTracker_go){ |
Marieke | 0:818fc79663f2 | 156 | TimeTracker_go=false; |
Marieke | 0:818fc79663f2 | 157 | TimeTrackerF(); |
Marieke | 0:818fc79663f2 | 158 | } |
Marieke | 2:915abd97ac16 | 159 | if (sampleT_go){ |
Marieke | 0:818fc79663f2 | 160 | sampleT_go=false; |
Marieke | 0:818fc79663f2 | 161 | sample(); |
Marieke | 0:818fc79663f2 | 162 | }*/ |
Marieke | 0:818fc79663f2 | 163 | } |
Marieke | 0:818fc79663f2 | 164 | } |