All working, HIDScope working with BiQuads! Filter coefficients are not perfect yet.
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of prog_practPutty2 by
Revision 0:818fc79663f2, committed 2016-10-11
- Comitter:
- Marieke
- Date:
- Tue Oct 11 11:24:52 2016 +0000
- Child:
- 1:812a1637b6cb
- Commit message:
- Counts and Derivative in Putty working!
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MODSERIAL.lib Tue Oct 11 11:24:52 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/Sissors/code/MODSERIAL/#4737f8a5b018
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/QEI_DCmotors.lib Tue Oct 11 11:24:52 2016 +0000 @@ -0,0 +1,1 @@ +http://developer.mbed.org/users/aberk/code/QEI/#5c2ad81551aa
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Oct 11 11:24:52 2016 +0000 @@ -0,0 +1,145 @@ +#include "mbed.h" +#include <math.h> +#include "MODSERIAL.h" +#include "QEI.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, sampleT, TimeTracker; +int counts; +float DerivativeCounts; +int countsPrev = 0; + +float referenceVelocity = 0; + +volatile bool MeasureTicker_go=false, TimeTracker_go=false, sampleT_go=false; + +void MeasureTicker_act(){MeasureTicker_go=true;}; // Activates go-flags +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); + //int countsPrev = 0; + /* QEI Encoder(D12, D13, NC, 32); // turns on encoder + counts = Encoder.getPulses(); // gives position + DerivativeCounts = ((float) counts-countsPrev)/0.01; + countsPrev = counts; + pc.printf("Counts: %i rad/s \r\n", counts); + pc.printf("Derivative Counts: %d rad/s \r\n", DerivativeCounts);*/ +} + +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); +}*/ + +int main() +{ + //Initialize + led1=0; + led2=0; + float Potmeter = potMeterIn.read(); + MeasureTicker.attach(&MeasureTicker_act, 0.01f); + 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 = ((float) counts-countsPrev)/0.01; + countsPrev = counts; + pc.printf("Counts: %i rad/s \r\n", counts); + pc.printf("Derivative Counts: %f rad/s \r\n", DerivativeCounts); + } + if (TimeTracker_go){ + TimeTracker_go=false; + TimeTrackerF(); + } + /*if (sampleT_go){ + sampleT_go=false; + sample(); + }*/ + } +} \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Tue Oct 11 11:24:52 2016 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/25aea2a3f4e3 \ No newline at end of file