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@1:f26a53da33ed, 2016-10-10 (annotated)

Committer:
IngmarLoohuis
Date:
Mon Oct 10 13:39:01 2016 +0000
Revision:
1:f26a53da33ed
Parent:
0:2f40eb89ffce
Child:
2:665df4abd084
Draaien werkt, Draairichting niet

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IngmarLoohuis 0:2f40eb89ffce 1 #include "mbed.h"
IngmarLoohuis 0:2f40eb89ffce 2 #include "QEI.h"
IngmarLoohuis 0:2f40eb89ffce 3 #include "math.h"
IngmarLoohuis 0:2f40eb89ffce 4
IngmarLoohuis 1:f26a53da33ed 5
IngmarLoohuis 1:f26a53da33ed 6 InterruptIn button1(D3);
IngmarLoohuis 1:f26a53da33ed 7 AnalogIn potMeterIn(A5);
IngmarLoohuis 1:f26a53da33ed 8 DigitalOut motor1DirectionPin(D4);
IngmarLoohuis 1:f26a53da33ed 9 PwmOut motor1MagnitudePin(D5);
IngmarLoohuis 1:f26a53da33ed 10
IngmarLoohuis 1:f26a53da33ed 11 Ticker foutprinter1;
IngmarLoohuis 1:f26a53da33ed 12 Ticker foutprinter2;
IngmarLoohuis 1:f26a53da33ed 13 Ticker callMotor;
IngmarLoohuis 1:f26a53da33ed 14 Serial pc(USBTX,USBRX);
IngmarLoohuis 0:2f40eb89ffce 15
IngmarLoohuis 1:f26a53da33ed 16 volatile float motorValue=0.0;
IngmarLoohuis 1:f26a53da33ed 17 volatile float referenceVelocity=0.0; // in rad/s
IngmarLoohuis 1:f26a53da33ed 18 const float maxVelocity=8.4; // in rad/s of course!
IngmarLoohuis 1:f26a53da33ed 19 const float MotorGain=8.4; // unit: (rad/s) / PWM
IngmarLoohuis 1:f26a53da33ed 20
IngmarLoohuis 1:f26a53da33ed 21
IngmarLoohuis 1:f26a53da33ed 22 void foutprint1()
IngmarLoohuis 1:f26a53da33ed 23 {
IngmarLoohuis 1:f26a53da33ed 24 pc.printf("Draairichting = %i\n\r", motor1DirectionPin);
IngmarLoohuis 1:f26a53da33ed 25 }
IngmarLoohuis 1:f26a53da33ed 26
IngmarLoohuis 1:f26a53da33ed 27
IngmarLoohuis 1:f26a53da33ed 28
IngmarLoohuis 1:f26a53da33ed 29 void foutprint2()
IngmarLoohuis 1:f26a53da33ed 30 {
IngmarLoohuis 1:f26a53da33ed 31 pc.printf("Ref Vel = %f\n\r", referenceVelocity) ;
IngmarLoohuis 1:f26a53da33ed 32 }
IngmarLoohuis 1:f26a53da33ed 33
IngmarLoohuis 1:f26a53da33ed 34
IngmarLoohuis 0:2f40eb89ffce 35
IngmarLoohuis 0:2f40eb89ffce 36 float getReferenceVelocity()
IngmarLoohuis 0:2f40eb89ffce 37 {
IngmarLoohuis 0:2f40eb89ffce 38 // Returns reference velocity in rad/s.
IngmarLoohuis 0:2f40eb89ffce 39 // Positive value means clockwise rotation.
IngmarLoohuis 0:2f40eb89ffce 40 if (button1)
IngmarLoohuis 1:f26a53da33ed 41 {
IngmarLoohuis 1:f26a53da33ed 42 // Clockwise rotation
IngmarLoohuis 1:f26a53da33ed 43 referenceVelocity = potMeterIn * maxVelocity;
IngmarLoohuis 1:f26a53da33ed 44 }
IngmarLoohuis 1:f26a53da33ed 45 else
IngmarLoohuis 1:f26a53da33ed 46 {
IngmarLoohuis 0:2f40eb89ffce 47 // Counterclockwise rotation
IngmarLoohuis 0:2f40eb89ffce 48 referenceVelocity = -1*potMeterIn * maxVelocity;
IngmarLoohuis 1:f26a53da33ed 49 }
IngmarLoohuis 0:2f40eb89ffce 50 return referenceVelocity;
IngmarLoohuis 0:2f40eb89ffce 51 }
IngmarLoohuis 0:2f40eb89ffce 52
IngmarLoohuis 0:2f40eb89ffce 53 float FeedForwardControl(float referenceVelocity)
IngmarLoohuis 0:2f40eb89ffce 54 {
IngmarLoohuis 0:2f40eb89ffce 55 // very simple linear feed-forward control
IngmarLoohuis 1:f26a53da33ed 56 motorValue = referenceVelocity / MotorGain;
IngmarLoohuis 0:2f40eb89ffce 57 return motorValue;
IngmarLoohuis 0:2f40eb89ffce 58 }
IngmarLoohuis 0:2f40eb89ffce 59
IngmarLoohuis 0:2f40eb89ffce 60 void SetMotor1(float motorValue)
IngmarLoohuis 0:2f40eb89ffce 61 {
IngmarLoohuis 0:2f40eb89ffce 62 // Given -1<=motorValue<=1, this sets the PWM and direction
IngmarLoohuis 0:2f40eb89ffce 63 // bits for motor 1. Positive value makes motor rotating
IngmarLoohuis 0:2f40eb89ffce 64 // clockwise. motorValues outside range are truncated to
IngmarLoohuis 0:2f40eb89ffce 65 // within range
IngmarLoohuis 1:f26a53da33ed 66 if (motorValue >=0.0) motor1DirectionPin=1;
IngmarLoohuis 1:f26a53da33ed 67 else motor1DirectionPin=0;
IngmarLoohuis 1:f26a53da33ed 68 if (fabs(motorValue)>1) motor1MagnitudePin = 1;
IngmarLoohuis 1:f26a53da33ed 69 else motor1MagnitudePin = fabs(motorValue);
IngmarLoohuis 0:2f40eb89ffce 70 }
IngmarLoohuis 0:2f40eb89ffce 71
IngmarLoohuis 0:2f40eb89ffce 72 void MeasureAndControl(void)
IngmarLoohuis 0:2f40eb89ffce 73 {
IngmarLoohuis 0:2f40eb89ffce 74 // This function measures the potmeter position, extracts a
IngmarLoohuis 0:2f40eb89ffce 75 // reference velocity from it, and controls the motor with
IngmarLoohuis 0:2f40eb89ffce 76 // a simple FeedForward controller. Call this from a Ticker.
IngmarLoohuis 1:f26a53da33ed 77 referenceVelocity = getReferenceVelocity();
IngmarLoohuis 1:f26a53da33ed 78 motorValue = FeedForwardControl(referenceVelocity);
IngmarLoohuis 0:2f40eb89ffce 79 SetMotor1(motorValue);
IngmarLoohuis 0:2f40eb89ffce 80 }
IngmarLoohuis 0:2f40eb89ffce 81
IngmarLoohuis 0:2f40eb89ffce 82 int main()
IngmarLoohuis 1:f26a53da33ed 83 {
IngmarLoohuis 1:f26a53da33ed 84 motor1MagnitudePin.period(1.0);
IngmarLoohuis 1:f26a53da33ed 85 foutprinter1.attach(foutprint1,1.0f);
IngmarLoohuis 1:f26a53da33ed 86 foutprinter2.attach(foutprint2,1.0f);
IngmarLoohuis 1:f26a53da33ed 87 callMotor.attach(MeasureAndControl, 0.1f);
IngmarLoohuis 1:f26a53da33ed 88 pc.baud(115200);
IngmarLoohuis 1:f26a53da33ed 89 while (true) {
IngmarLoohuis 0:2f40eb89ffce 90 }
IngmarLoohuis 0:2f40eb89ffce 91 }
IngmarLoohuis 0:2f40eb89ffce 92