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
- Committer:
- GerhardBerman
- Date:
- 2016-10-17
- Revision:
- 5:0bc7a874c640
- Parent:
- 4:7224b1f0c668
File content as of revision 5:0bc7a874c640:
#include "mbed.h" #include <math.h> #include "MODSERIAL.h" #include "QEI.h" #include "HIDScope.h" #include "BiQuad.h" DigitalIn encoder1A (D13); //Channel A van Encoder 1 DigitalIn encoder1B (D12); //Channel B van Encoder 1 DigitalOut led1 (D11); DigitalOut led2 (D10); AnalogIn potMeterIn(A0); DigitalOut motor1DirectionPin(D7); PwmOut motor1MagnitudePin(D6); DigitalIn button1(D5); Serial pc(USBTX,USBRX); Ticker MeasureTicker, BiQuadTicker;// sampleT, TimeTracker; HIDScope scope(2); int counts; double DerivativeCounts; int countsPrev = 0; float referenceVelocity = 0; double bqcDerivativeCounts = 0; BiQuadChain bqc; BiQuad bq1(0.0048,0.0193,0.0289,0.0193,0.0048); //get numbers from butter filter MATLAB BiQuad bq2(1.0000,-2.3695,2.3140,-1.0547,0.1874); volatile bool MeasureTicker_go=false, BiQuadTicker_go=false;// TimeTracker_go=false, sampleT_go=false; void MeasureTicker_act(){MeasureTicker_go=true;}; // Activates go-flags void BiQuadTicker_act(){BiQuadTicker_go=true;}; //void TimeTracker_act(){TimeTracker_go=true;}; //void sampleT_act(){sampleT_go=true;}; float GetReferenceVelocity() { // Returns reference velocity in rad/s. // Positive value means clockwise rotation. const float maxVelocity = 8.4; // in rad/s of course! if (button1 == 0){ led1=1; led2=0; // Counterclockwise rotation referenceVelocity = potMeterIn * maxVelocity; } else { led1=0; led2=1; // Clockwise rotation referenceVelocity = -1*potMeterIn * maxVelocity; } return referenceVelocity; } float FeedForwardControl(float referenceVelocity) { // very simple linear feed-forward control const float MotorGain=8.4; // unit: (rad/s) / PWM float motorValue = referenceVelocity / MotorGain; return motorValue; } void SetMotor1(float motorValue) { // Given -1<=motorValue<=1, this sets the PWM and direction // bits for motor 1. Positive value makes motor rotating // clockwise. motorValues outside range are truncated to // within range if (motorValue >=0) motor1DirectionPin=1; else motor1DirectionPin=0; if (fabs(motorValue)>1) motor1MagnitudePin = 1; else motor1MagnitudePin = fabs(motorValue); } void MeasureAndControl() { // This function measures the potmeter position, extracts a // reference velocity from it, and controls the motor with // a simple FeedForward controller. Call this from a Ticker. float referenceVelocity = GetReferenceVelocity(); float motorValue = FeedForwardControl(referenceVelocity); SetMotor1(motorValue); } void TimeTrackerF(){ wait(1); float Potmeter = potMeterIn.read(); pc.printf("Reference velocity: %f rad/s \r\n", referenceVelocity); pc.printf("Potmeter: %f rad/s \r\n", Potmeter); //pc.printf("Counts: %i rad/s \r\n", counts); //pc.printf("Derivative Counts: %i rad/s \r\n", DerivativeCounts); } /* 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(){ //double in=DerivativeCounts(); bqcDerivativeCounts=bqc.step(DerivativeCounts); //return(bqcDerivativeCounts); } int main() { //Initialize led1=1; led2=1; 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, 0.1f); pc.baud(115200); 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) { if (MeasureTicker_go){ MeasureTicker_go=false; MeasureAndControl(); // 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 (TimeTracker_go){ TimeTracker_go=false; TimeTrackerF(); } if (sampleT_go){ sampleT_go=false; sample(); }*/ } }