Ingmar Loohuis / Mbed 2 deprecated MotorControl

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

main.cpp@2:665df4abd084, 2016-10-17 (annotated)

Committer:
IngmarLoohuis
Date:
Mon Oct 17 12:53:03 2016 +0000
Revision:
2:665df4abd084
Parent:
1:f26a53da33ed
Child:
3:1ab2d2b1705f
Draaien+HIDScope werkt

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IngmarLoohuis 0:2f40eb89ffce 1 #include "mbed.h"
IngmarLoohuis 2:665df4abd084 2 #include "MODSERIAL.h"
IngmarLoohuis 0:2f40eb89ffce 3 #include "QEI.h"
IngmarLoohuis 0:2f40eb89ffce 4 #include "math.h"
IngmarLoohuis 2:665df4abd084 5 #include "HIDScope.h"
IngmarLoohuis 1:f26a53da33ed 6
IngmarLoohuis 2:665df4abd084 7 //Defining ports
IngmarLoohuis 2:665df4abd084 8 DigitalOut motor1DirectionPin (D4);
IngmarLoohuis 1:f26a53da33ed 9 PwmOut motor1MagnitudePin(D5);
IngmarLoohuis 2:665df4abd084 10 DigitalIn button(D2);
IngmarLoohuis 2:665df4abd084 11 AnalogIn potmeter(A0);
IngmarLoohuis 1:f26a53da33ed 12 Serial pc(USBTX,USBRX);
IngmarLoohuis 2:665df4abd084 13 QEI Encoder(D12,D13,NC,32);
IngmarLoohuis 2:665df4abd084 14 HIDScope scope(1);
IngmarLoohuis 1:f26a53da33ed 15
IngmarLoohuis 2:665df4abd084 16 // Setting tickers and printers
IngmarLoohuis 2:665df4abd084 17 Ticker tick;
IngmarLoohuis 2:665df4abd084 18 Ticker callMotor;
IngmarLoohuis 2:665df4abd084 19 Ticker pos;
IngmarLoohuis 2:665df4abd084 20 Ticker encoderTicker;
IngmarLoohuis 1:f26a53da33ed 21
IngmarLoohuis 2:665df4abd084 22 const float pi = 3.14159265359;
IngmarLoohuis 2:665df4abd084 23 const float ts = 1.0/1000.0;
IngmarLoohuis 2:665df4abd084 24 const int VELOCITY_CHANNEL = 0;
IngmarLoohuis 0:2f40eb89ffce 25
IngmarLoohuis 2:665df4abd084 26 //Get reference velocity
IngmarLoohuis 2:665df4abd084 27 float GetReferenceVelocity() {
IngmarLoohuis 2:665df4abd084 28 // Returns reference velocity in rad/s.
IngmarLoohuis 0:2f40eb89ffce 29 // Positive value means clockwise rotation.
IngmarLoohuis 2:665df4abd084 30 const float maxVelocity = 8.4; //Als de potmeter van 0 tot 1 gaat (als die maar tot 0.25 gaat, dan max velocity 4x zo groot als motorgain maken
IngmarLoohuis 2:665df4abd084 31 volatile float referenceVelocity; // in rad/s
IngmarLoohuis 2:665df4abd084 32 if (button) //nog even kijken voor wanneer die + en - is
IngmarLoohuis 2:665df4abd084 33 {
IngmarLoohuis 2:665df4abd084 34 // Clockwise rotation
IngmarLoohuis 2:665df4abd084 35 referenceVelocity = potmeter * maxVelocity;
IngmarLoohuis 2:665df4abd084 36 }
IngmarLoohuis 1:f26a53da33ed 37 else
IngmarLoohuis 2:665df4abd084 38 {
IngmarLoohuis 0:2f40eb89ffce 39 // Counterclockwise rotation
IngmarLoohuis 2:665df4abd084 40 referenceVelocity = -1*potmeter * maxVelocity;
IngmarLoohuis 2:665df4abd084 41 }
IngmarLoohuis 0:2f40eb89ffce 42 return referenceVelocity;
IngmarLoohuis 0:2f40eb89ffce 43 }
IngmarLoohuis 0:2f40eb89ffce 44
IngmarLoohuis 2:665df4abd084 45 void SetMotor1(float motorValue) {
IngmarLoohuis 2:665df4abd084 46 //Given -1<=motorValue<=1, this sets the PWM and direction
IngmarLoohuis 0:2f40eb89ffce 47 // bits for motor 1. Positive value makes motor rotating
IngmarLoohuis 0:2f40eb89ffce 48 // clockwise. motorValues outside range are truncated to
IngmarLoohuis 0:2f40eb89ffce 49 // within range
IngmarLoohuis 2:665df4abd084 50 if (motorValue >=0)
IngmarLoohuis 2:665df4abd084 51 {
IngmarLoohuis 2:665df4abd084 52 motor1DirectionPin=1;
IngmarLoohuis 2:665df4abd084 53 }
IngmarLoohuis 2:665df4abd084 54 else
IngmarLoohuis 2:665df4abd084 55 {
IngmarLoohuis 2:665df4abd084 56 motor1DirectionPin=0;
IngmarLoohuis 2:665df4abd084 57 }
IngmarLoohuis 2:665df4abd084 58 if (fabs(motorValue)>1)
IngmarLoohuis 2:665df4abd084 59 {
IngmarLoohuis 2:665df4abd084 60 motor1MagnitudePin = 1;
IngmarLoohuis 2:665df4abd084 61 }
IngmarLoohuis 2:665df4abd084 62 else
IngmarLoohuis 2:665df4abd084 63 {
IngmarLoohuis 2:665df4abd084 64 motor1MagnitudePin = fabs(motorValue);
IngmarLoohuis 2:665df4abd084 65 }
IngmarLoohuis 2:665df4abd084 66 }
IngmarLoohuis 2:665df4abd084 67
IngmarLoohuis 2:665df4abd084 68 float FeedForwardControl(float referenceVelocity) {
IngmarLoohuis 2:665df4abd084 69 // very simple linear feed-forward control
IngmarLoohuis 2:665df4abd084 70 const float MotorGain=8.4; // unit: (rad/s) / PWM
IngmarLoohuis 2:665df4abd084 71 float motorValue = referenceVelocity / MotorGain;
IngmarLoohuis 2:665df4abd084 72 return motorValue;
IngmarLoohuis 0:2f40eb89ffce 73 }
IngmarLoohuis 0:2f40eb89ffce 74
IngmarLoohuis 2:665df4abd084 75 void MeasureAndControl(void) {
IngmarLoohuis 0:2f40eb89ffce 76 // This function measures the potmeter position, extracts a
IngmarLoohuis 2:665df4abd084 77 // reference velocity from it, and controls the motor with
IngmarLoohuis 0:2f40eb89ffce 78 // a simple FeedForward controller. Call this from a Ticker.
IngmarLoohuis 2:665df4abd084 79 float referenceVelocity = GetReferenceVelocity();
IngmarLoohuis 2:665df4abd084 80 float motorValue = FeedForwardControl(referenceVelocity);
IngmarLoohuis 0:2f40eb89ffce 81 SetMotor1(motorValue);
IngmarLoohuis 0:2f40eb89ffce 82 }
IngmarLoohuis 2:665df4abd084 83
IngmarLoohuis 2:665df4abd084 84
IngmarLoohuis 2:665df4abd084 85 float getPosition() {
IngmarLoohuis 2:665df4abd084 86 //getting the position of the motor to take derivative to have a
IngmarLoohuis 2:665df4abd084 87 //different velocity signal.
IngmarLoohuis 2:665df4abd084 88 const float ts = 1.0/1000.0;
IngmarLoohuis 2:665df4abd084 89 volatile float pulses = Encoder.getPulses();
IngmarLoohuis 2:665df4abd084 90 volatile float revolutions = Encoder.getRevolutions();
IngmarLoohuis 2:665df4abd084 91 volatile float position = (pulses / 2 * 32) * 360; //formula from QEI library
IngmarLoohuis 2:665df4abd084 92 return position;
IngmarLoohuis 2:665df4abd084 93 }
IngmarLoohuis 0:2f40eb89ffce 94
IngmarLoohuis 2:665df4abd084 95 void encoderTick() {
IngmarLoohuis 2:665df4abd084 96 int pulses = Encoder.getPulses();
IngmarLoohuis 2:665df4abd084 97
IngmarLoohuis 2:665df4abd084 98 //calculate the total angle that is traveled so far.
IngmarLoohuis 2:665df4abd084 99 double radians = (pulses / ts) * ((2*pi)/32);
IngmarLoohuis 2:665df4abd084 100
IngmarLoohuis 2:665df4abd084 101 //update lastEncoderRead such that it can be used for next time
IngmarLoohuis 2:665df4abd084 102 double lastEncoderRead = radians;
IngmarLoohuis 2:665df4abd084 103
IngmarLoohuis 2:665df4abd084 104 //approximate the derivative for the angular velocity.
IngmarLoohuis 2:665df4abd084 105 double xdif = radians - lastEncoderRead;
IngmarLoohuis 2:665df4abd084 106 double xderiv = xdif / ts;
IngmarLoohuis 2:665df4abd084 107
IngmarLoohuis 2:665df4abd084 108
IngmarLoohuis 2:665df4abd084 109
IngmarLoohuis 2:665df4abd084 110 //send velocity to HIDScope
IngmarLoohuis 2:665df4abd084 111 scope.set(VELOCITY_CHANNEL, radians);
IngmarLoohuis 2:665df4abd084 112 scope.send();
IngmarLoohuis 2:665df4abd084 113 }
IngmarLoohuis 2:665df4abd084 114
IngmarLoohuis 2:665df4abd084 115
IngmarLoohuis 2:665df4abd084 116
IngmarLoohuis 0:2f40eb89ffce 117 int main()
IngmarLoohuis 2:665df4abd084 118 {
IngmarLoohuis 2:665df4abd084 119 encoderTicker.attach(encoderTick, 1.0/100000.0);
IngmarLoohuis 2:665df4abd084 120 motor1MagnitudePin.period(1.0/100000.0);
IngmarLoohuis 2:665df4abd084 121 callMotor.attach(MeasureAndControl,1);
IngmarLoohuis 2:665df4abd084 122 pc.baud(115200);
IngmarLoohuis 2:665df4abd084 123 //tick.attach(print,1);
IngmarLoohuis 2:665df4abd084 124 while(true){}
IngmarLoohuis 2:665df4abd084 125 }