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:
Marieke
Date:
Tue Oct 11 11:24:52 2016 +0000
Revision:
0:818fc79663f2
Child:
1:812a1637b6cb
Counts and Derivative in Putty working!

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 0:818fc79663f2 5
Marieke 0:818fc79663f2 6 DigitalIn encoder1A (D13); //Channel A van Encoder 1
Marieke 0:818fc79663f2 7 DigitalIn encoder1B (D12); //Channel B van Encoder 1
Marieke 0:818fc79663f2 8 DigitalOut led1 (D11);
Marieke 0:818fc79663f2 9 DigitalOut led2 (D10);
Marieke 0:818fc79663f2 10 AnalogIn potMeterIn(A0);
Marieke 0:818fc79663f2 11 DigitalOut motor1DirectionPin(D7);
Marieke 0:818fc79663f2 12 PwmOut motor1MagnitudePin(D6);
Marieke 0:818fc79663f2 13 DigitalIn button1(D5);
Marieke 0:818fc79663f2 14
Marieke 0:818fc79663f2 15
Marieke 0:818fc79663f2 16 Serial pc(USBTX,USBRX);
Marieke 0:818fc79663f2 17 Ticker MeasureTicker, sampleT, TimeTracker;
Marieke 0:818fc79663f2 18 int counts;
Marieke 0:818fc79663f2 19 float DerivativeCounts;
Marieke 0:818fc79663f2 20 int countsPrev = 0;
Marieke 0:818fc79663f2 21
Marieke 0:818fc79663f2 22 float referenceVelocity = 0;
Marieke 0:818fc79663f2 23
Marieke 0:818fc79663f2 24 volatile bool MeasureTicker_go=false, TimeTracker_go=false, sampleT_go=false;
Marieke 0:818fc79663f2 25
Marieke 0:818fc79663f2 26 void MeasureTicker_act(){MeasureTicker_go=true;}; // Activates go-flags
Marieke 0:818fc79663f2 27 void TimeTracker_act(){TimeTracker_go=true;};
Marieke 0:818fc79663f2 28 void sampleT_act(){sampleT_go=true;};
Marieke 0:818fc79663f2 29
Marieke 0:818fc79663f2 30 float GetReferenceVelocity()
Marieke 0:818fc79663f2 31 {
Marieke 0:818fc79663f2 32 // Returns reference velocity in rad/s.
Marieke 0:818fc79663f2 33 // Positive value means clockwise rotation.
Marieke 0:818fc79663f2 34 const float maxVelocity = 8.4; // in rad/s of course!
Marieke 0:818fc79663f2 35 if (button1 == 0){
Marieke 0:818fc79663f2 36 led1=1;
Marieke 0:818fc79663f2 37 led2=0;
Marieke 0:818fc79663f2 38 // Counterclockwise rotation
Marieke 0:818fc79663f2 39 referenceVelocity = potMeterIn * maxVelocity;
Marieke 0:818fc79663f2 40 }
Marieke 0:818fc79663f2 41 else {
Marieke 0:818fc79663f2 42 led1=0;
Marieke 0:818fc79663f2 43 led2=1;
Marieke 0:818fc79663f2 44 // Clockwise rotation
Marieke 0:818fc79663f2 45 referenceVelocity = -1*potMeterIn * maxVelocity;
Marieke 0:818fc79663f2 46 }
Marieke 0:818fc79663f2 47 return referenceVelocity;
Marieke 0:818fc79663f2 48 }
Marieke 0:818fc79663f2 49
Marieke 0:818fc79663f2 50 float FeedForwardControl(float referenceVelocity)
Marieke 0:818fc79663f2 51 {
Marieke 0:818fc79663f2 52 // very simple linear feed-forward control
Marieke 0:818fc79663f2 53 const float MotorGain=8.4; // unit: (rad/s) / PWM
Marieke 0:818fc79663f2 54 float motorValue = referenceVelocity / MotorGain;
Marieke 0:818fc79663f2 55 return motorValue;
Marieke 0:818fc79663f2 56 }
Marieke 0:818fc79663f2 57
Marieke 0:818fc79663f2 58 void SetMotor1(float motorValue)
Marieke 0:818fc79663f2 59 {
Marieke 0:818fc79663f2 60 // Given -1<=motorValue<=1, this sets the PWM and direction
Marieke 0:818fc79663f2 61 // bits for motor 1. Positive value makes motor rotating
Marieke 0:818fc79663f2 62 // clockwise. motorValues outside range are truncated to
Marieke 0:818fc79663f2 63 // within range
Marieke 0:818fc79663f2 64 if (motorValue >=0) motor1DirectionPin=1;
Marieke 0:818fc79663f2 65 else motor1DirectionPin=0;
Marieke 0:818fc79663f2 66 if (fabs(motorValue)>1) motor1MagnitudePin = 1;
Marieke 0:818fc79663f2 67 else motor1MagnitudePin = fabs(motorValue);
Marieke 0:818fc79663f2 68 }
Marieke 0:818fc79663f2 69
Marieke 0:818fc79663f2 70 void MeasureAndControl()
Marieke 0:818fc79663f2 71 {
Marieke 0:818fc79663f2 72 // This function measures the potmeter position, extracts a
Marieke 0:818fc79663f2 73 // reference velocity from it, and controls the motor with
Marieke 0:818fc79663f2 74 // a simple FeedForward controller. Call this from a Ticker.
Marieke 0:818fc79663f2 75 float referenceVelocity = GetReferenceVelocity();
Marieke 0:818fc79663f2 76 float motorValue = FeedForwardControl(referenceVelocity);
Marieke 0:818fc79663f2 77 SetMotor1(motorValue);
Marieke 0:818fc79663f2 78 //int countsPrev = 0;
Marieke 0:818fc79663f2 79 /* QEI Encoder(D12, D13, NC, 32); // turns on encoder
Marieke 0:818fc79663f2 80 counts = Encoder.getPulses(); // gives position
Marieke 0:818fc79663f2 81 DerivativeCounts = ((float) counts-countsPrev)/0.01;
Marieke 0:818fc79663f2 82 countsPrev = counts;
Marieke 0:818fc79663f2 83 pc.printf("Counts: %i rad/s \r\n", counts);
Marieke 0:818fc79663f2 84 pc.printf("Derivative Counts: %d rad/s \r\n", DerivativeCounts);*/
Marieke 0:818fc79663f2 85 }
Marieke 0:818fc79663f2 86
Marieke 0:818fc79663f2 87 void TimeTrackerF(){
Marieke 0:818fc79663f2 88 wait(1);
Marieke 0:818fc79663f2 89 float Potmeter = potMeterIn.read();
Marieke 0:818fc79663f2 90 pc.printf("Reference velocity: %f rad/s \r\n", referenceVelocity);
Marieke 0:818fc79663f2 91 pc.printf("Potmeter: %f rad/s \r\n", Potmeter);
Marieke 0:818fc79663f2 92 //pc.printf("Counts: %i rad/s \r\n", counts);
Marieke 0:818fc79663f2 93 //pc.printf("Derivative Counts: %i rad/s \r\n", DerivativeCounts);
Marieke 0:818fc79663f2 94 }
Marieke 0:818fc79663f2 95 /*
Marieke 0:818fc79663f2 96 void sample()
Marieke 0:818fc79663f2 97 {
Marieke 0:818fc79663f2 98 int countsPrev = 0;
Marieke 0:818fc79663f2 99 QEI Encoder(D12, D13, NC, 32);
Marieke 0:818fc79663f2 100 counts = Encoder.getPulses(); // gives position
Marieke 0:818fc79663f2 101 //scope.set(0,counts);
Marieke 0:818fc79663f2 102 DerivativeCounts = (counts-countsPrev)/0.001;
Marieke 0:818fc79663f2 103 //scope.set(1,DerivativeCounts);
Marieke 0:818fc79663f2 104 countsPrev = counts;
Marieke 0:818fc79663f2 105 //scope.send();
Marieke 0:818fc79663f2 106 pc.printf("Counts: %i rad/s \r\n", counts);
Marieke 0:818fc79663f2 107 pc.printf("Derivative Counts: %d rad/s \r\n", DerivativeCounts);
Marieke 0:818fc79663f2 108 }*/
Marieke 0:818fc79663f2 109
Marieke 0:818fc79663f2 110 int main()
Marieke 0:818fc79663f2 111 {
Marieke 0:818fc79663f2 112 //Initialize
Marieke 0:818fc79663f2 113 led1=0;
Marieke 0:818fc79663f2 114 led2=0;
Marieke 0:818fc79663f2 115 float Potmeter = potMeterIn.read();
Marieke 0:818fc79663f2 116 MeasureTicker.attach(&MeasureTicker_act, 0.01f);
Marieke 0:818fc79663f2 117 TimeTracker.attach(&TimeTracker_act, 0.1f);
Marieke 0:818fc79663f2 118 pc.baud(115200);
Marieke 0:818fc79663f2 119 QEI Encoder(D12, D13, NC, 32); // turns on encoder
Marieke 0:818fc79663f2 120 //sampleT.attach(&sampleT_act, 0.1f);
Marieke 0:818fc79663f2 121 pc.printf("Reference velocity: %f rad/s \r\n", referenceVelocity);
Marieke 0:818fc79663f2 122 pc.printf("Potmeter: %f rad/s \r\n", Potmeter);
Marieke 0:818fc79663f2 123
Marieke 0:818fc79663f2 124 while(1)
Marieke 0:818fc79663f2 125 {
Marieke 0:818fc79663f2 126 if (MeasureTicker_go){
Marieke 0:818fc79663f2 127 MeasureTicker_go=false;
Marieke 0:818fc79663f2 128 MeasureAndControl();
Marieke 0:818fc79663f2 129 // Encoder part
Marieke 0:818fc79663f2 130 counts = Encoder.getPulses(); // gives position
Marieke 0:818fc79663f2 131 DerivativeCounts = ((float) counts-countsPrev)/0.01;
Marieke 0:818fc79663f2 132 countsPrev = counts;
Marieke 0:818fc79663f2 133 pc.printf("Counts: %i rad/s \r\n", counts);
Marieke 0:818fc79663f2 134 pc.printf("Derivative Counts: %f rad/s \r\n", DerivativeCounts);
Marieke 0:818fc79663f2 135 }
Marieke 0:818fc79663f2 136 if (TimeTracker_go){
Marieke 0:818fc79663f2 137 TimeTracker_go=false;
Marieke 0:818fc79663f2 138 TimeTrackerF();
Marieke 0:818fc79663f2 139 }
Marieke 0:818fc79663f2 140 /*if (sampleT_go){
Marieke 0:818fc79663f2 141 sampleT_go=false;
Marieke 0:818fc79663f2 142 sample();
Marieke 0:818fc79663f2 143 }*/
Marieke 0:818fc79663f2 144 }
Marieke 0:818fc79663f2 145 }