All working, HIDScope working with BiQuads! Filter coefficients are not perfect yet.

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of prog_practPutty2 by Gerhard Berman

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?

UserRevisionLine numberNew 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 }