Jan Boerman / Mbed 2 deprecated MotorPotButtonEncoderVelocity

Exercise #2 -- show the angular velocity in HIDScope

Dependencies:   HIDScope QEI mbed

main.cpp@0:35f03aa18046, 2016-10-07 (annotated)

Committer:
Jankoekenpan
Date:
Fri Oct 07 15:39:45 2016 +0000
Revision:
0:35f03aa18046
initial commit. program compiles but doesn't seem to be working.;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jankoekenpan 0:35f03aa18046 1 #include "mbed.h"
Jankoekenpan 0:35f03aa18046 2 #include "math.h"
Jankoekenpan 0:35f03aa18046 3 #include "HIDScope.h"
Jankoekenpan 0:35f03aa18046 4 #include "QEI.h"
Jankoekenpan 0:35f03aa18046 5
Jankoekenpan 0:35f03aa18046 6 // ------ Hardware Interfaces -------
Jankoekenpan 0:35f03aa18046 7
Jankoekenpan 0:35f03aa18046 8 const PinName motor1dir = D7;
Jankoekenpan 0:35f03aa18046 9 const PinName motor1PWM = D6;
Jankoekenpan 0:35f03aa18046 10 DigitalOut motor1direction(motor1dir);
Jankoekenpan 0:35f03aa18046 11 PwmOut motor1control(motor1PWM);
Jankoekenpan 0:35f03aa18046 12
Jankoekenpan 0:35f03aa18046 13 const PinName button1name = D10; //for some reason D8 does not seem to work. wtf!
Jankoekenpan 0:35f03aa18046 14 const PinName pot1name = A0;
Jankoekenpan 0:35f03aa18046 15 InterruptIn button1(button1name);
Jankoekenpan 0:35f03aa18046 16 AnalogIn potMeterIn(pot1name);
Jankoekenpan 0:35f03aa18046 17
Jankoekenpan 0:35f03aa18046 18 const PinName encoderpin1 = D13;
Jankoekenpan 0:35f03aa18046 19 const PinName encoderpin2 = D12;
Jankoekenpan 0:35f03aa18046 20 QEI qei(encoderpin1, encoderpin2, NC, 32);
Jankoekenpan 0:35f03aa18046 21
Jankoekenpan 0:35f03aa18046 22 // ------- Objects used -------
Jankoekenpan 0:35f03aa18046 23
Jankoekenpan 0:35f03aa18046 24 // Ticker which controls the mother every 1/100 of a second.
Jankoekenpan 0:35f03aa18046 25 Ticker controlTicker;
Jankoekenpan 0:35f03aa18046 26
Jankoekenpan 0:35f03aa18046 27 Ticker debugTicker;
Jankoekenpan 0:35f03aa18046 28 Serial pc(USBTX, USBRX);
Jankoekenpan 0:35f03aa18046 29
Jankoekenpan 0:35f03aa18046 30 Ticker encoderTicker;
Jankoekenpan 0:35f03aa18046 31
Jankoekenpan 0:35f03aa18046 32 HIDScope scope(1);
Jankoekenpan 0:35f03aa18046 33
Jankoekenpan 0:35f03aa18046 34 // ------- Constants
Jankoekenpan 0:35f03aa18046 35
Jankoekenpan 0:35f03aa18046 36 const float motorGain = 8.4f;
Jankoekenpan 0:35f03aa18046 37
Jankoekenpan 0:35f03aa18046 38 const float MAX_VELOCITY = 8.4f; //radians per second
Jankoekenpan 0:35f03aa18046 39
Jankoekenpan 0:35f03aa18046 40 const bool CLOCKWISE = true;
Jankoekenpan 0:35f03aa18046 41
Jankoekenpan 0:35f03aa18046 42 //1 output rod revolution == 131.25 encoder revolution
Jankoekenpan 0:35f03aa18046 43 const float RATIO = 1.0f/131.25f;
Jankoekenpan 0:35f03aa18046 44
Jankoekenpan 0:35f03aa18046 45 const float tSample = 1.0f/1000.0f;
Jankoekenpan 0:35f03aa18046 46
Jankoekenpan 0:35f03aa18046 47 const double pi = 3.14159265358979323846;
Jankoekenpan 0:35f03aa18046 48
Jankoekenpan 0:35f03aa18046 49 const int VELOCITY_CHANNEL = 0;
Jankoekenpan 0:35f03aa18046 50
Jankoekenpan 0:35f03aa18046 51
Jankoekenpan 0:35f03aa18046 52 // ------ variables
Jankoekenpan 0:35f03aa18046 53
Jankoekenpan 0:35f03aa18046 54 volatile bool direction = CLOCKWISE;
Jankoekenpan 0:35f03aa18046 55
Jankoekenpan 0:35f03aa18046 56 volatile double lastEncoderRead = 0; //angle in radians.
Jankoekenpan 0:35f03aa18046 57
Jankoekenpan 0:35f03aa18046 58 // ------ functions
Jankoekenpan 0:35f03aa18046 59
Jankoekenpan 0:35f03aa18046 60 float getReferenceVelocity() {
Jankoekenpan 0:35f03aa18046 61 // Returns reference velocity in rad/s.
Jankoekenpan 0:35f03aa18046 62 return MAX_VELOCITY * potMeterIn.read();
Jankoekenpan 0:35f03aa18046 63 }
Jankoekenpan 0:35f03aa18046 64
Jankoekenpan 0:35f03aa18046 65 void setMotor1(float motorValue) {
Jankoekenpan 0:35f03aa18046 66 // Given motorValue<=1, writes the velocity to the pwm control.
Jankoekenpan 0:35f03aa18046 67 // MotorValues outside range are truncated to within range.
Jankoekenpan 0:35f03aa18046 68 motor1control.write(fabs(motorValue) > 1 ? 1 : fabs(motorValue));
Jankoekenpan 0:35f03aa18046 69 }
Jankoekenpan 0:35f03aa18046 70
Jankoekenpan 0:35f03aa18046 71 float feedForwardControl(float referenceVelocity) {
Jankoekenpan 0:35f03aa18046 72 // very simple linear feed-forward control
Jankoekenpan 0:35f03aa18046 73 // returns motorValue
Jankoekenpan 0:35f03aa18046 74 return referenceVelocity / motorGain;
Jankoekenpan 0:35f03aa18046 75 }
Jankoekenpan 0:35f03aa18046 76
Jankoekenpan 0:35f03aa18046 77 void measureAndControl(void) {
Jankoekenpan 0:35f03aa18046 78 // This function measures the potmeter position, extracts a
Jankoekenpan 0:35f03aa18046 79 // reference velocity from it, and controls the motor with
Jankoekenpan 0:35f03aa18046 80 // a simple FeedForward controller. Call this from a Ticker.
Jankoekenpan 0:35f03aa18046 81 float referenceVelocity = getReferenceVelocity();
Jankoekenpan 0:35f03aa18046 82 float motorValue = feedForwardControl(referenceVelocity);
Jankoekenpan 0:35f03aa18046 83 setMotor1(motorValue);
Jankoekenpan 0:35f03aa18046 84 }
Jankoekenpan 0:35f03aa18046 85
Jankoekenpan 0:35f03aa18046 86 void onButtonPress() {
Jankoekenpan 0:35f03aa18046 87 // reverses the direction
Jankoekenpan 0:35f03aa18046 88 motor1direction.write(direction = !direction);
Jankoekenpan 0:35f03aa18046 89 pc.printf("direction: %s\r\n\n", direction == CLOCKWISE ? "clockwise" : "counter clockwise");
Jankoekenpan 0:35f03aa18046 90 }
Jankoekenpan 0:35f03aa18046 91
Jankoekenpan 0:35f03aa18046 92 void onDebugTick() {
Jankoekenpan 0:35f03aa18046 93
Jankoekenpan 0:35f03aa18046 94 pc.printf("pot input: %f\r\n", potMeterIn.read());
Jankoekenpan 0:35f03aa18046 95 pc.printf("motorValue: %f\r\n", feedForwardControl(getReferenceVelocity()));
Jankoekenpan 0:35f03aa18046 96 pc.printf("\n\n\n");
Jankoekenpan 0:35f03aa18046 97
Jankoekenpan 0:35f03aa18046 98 }
Jankoekenpan 0:35f03aa18046 99
Jankoekenpan 0:35f03aa18046 100 void encoderTick() {
Jankoekenpan 0:35f03aa18046 101 int pulses = qei.getPulses();
Jankoekenpan 0:35f03aa18046 102
Jankoekenpan 0:35f03aa18046 103 //calculate the total angle that is travelel so far.
Jankoekenpan 0:35f03aa18046 104 double radians = (pulses / tSample) * ((2*pi)/32);
Jankoekenpan 0:35f03aa18046 105
Jankoekenpan 0:35f03aa18046 106 //approximate the derivative for the angular velocity.
Jankoekenpan 0:35f03aa18046 107 double xdif = radians - lastEncoderRead;
Jankoekenpan 0:35f03aa18046 108 double xderiv = xdif / tSample;
Jankoekenpan 0:35f03aa18046 109
Jankoekenpan 0:35f03aa18046 110 //TODO apply a BiQuard filter in between here.
Jankoekenpan 0:35f03aa18046 111
Jankoekenpan 0:35f03aa18046 112 //send velocity to HIDScope
Jankoekenpan 0:35f03aa18046 113 scope.set(VELOCITY_CHANNEL, xderiv);
Jankoekenpan 0:35f03aa18046 114 scope.send();
Jankoekenpan 0:35f03aa18046 115
Jankoekenpan 0:35f03aa18046 116 //update lastEncoderRead such that it can be used for next time
Jankoekenpan 0:35f03aa18046 117 lastEncoderRead = radians;
Jankoekenpan 0:35f03aa18046 118 }
Jankoekenpan 0:35f03aa18046 119
Jankoekenpan 0:35f03aa18046 120 int main()
Jankoekenpan 0:35f03aa18046 121 {
Jankoekenpan 0:35f03aa18046 122 pc.baud(115200);
Jankoekenpan 0:35f03aa18046 123 qei.reset();
Jankoekenpan 0:35f03aa18046 124
Jankoekenpan 0:35f03aa18046 125 encoderTicker.attach(&encoderTick, tSample);
Jankoekenpan 0:35f03aa18046 126 controlTicker.attach(&measureAndControl, 1.0f/100.0f);
Jankoekenpan 0:35f03aa18046 127 button1.fall(&onButtonPress);
Jankoekenpan 0:35f03aa18046 128
Jankoekenpan 0:35f03aa18046 129 debugTicker.attach(&onDebugTick, 1);
Jankoekenpan 0:35f03aa18046 130
Jankoekenpan 0:35f03aa18046 131 while (true);
Jankoekenpan 0:35f03aa18046 132 }