Robert Geels / Mbed 2 deprecated motur

-

Dependencies:   mbed

main.cpp@0:0af5376a3506, 2017-10-06 (annotated)

Committer:
PRG
Date:
Fri Oct 06 07:21:33 2017 +0000
Revision:
0:0af5376a3506
-

Who changed what in which revision?

UserRevisionLine numberNew contents of line
PRG 0:0af5376a3506 1 #include "mbed.h"
PRG 0:0af5376a3506 2 #include "math.h"
PRG 0:0af5376a3506 3
PRG 0:0af5376a3506 4 DigitalIn button1(D8);
PRG 0:0af5376a3506 5 AnalogIn potMeterIn(A1);
PRG 0:0af5376a3506 6
PRG 0:0af5376a3506 7 float motor1DirectionPin;
PRG 0:0af5376a3506 8 float motor1MagnitudePin;
PRG 0:0af5376a3506 9 float referenceVelocity; // in rad/s
PRG 0:0af5376a3506 10
PRG 0:0af5376a3506 11 float GetReferenceVelocity()
PRG 0:0af5376a3506 12 {
PRG 0:0af5376a3506 13 // Returns reference velocity in rad/s.
PRG 0:0af5376a3506 14 // Positive value means clockwise rotation.
PRG 0:0af5376a3506 15 const float maxVelocity=8.4; // in rad/s of course!
PRG 0:0af5376a3506 16
PRG 0:0af5376a3506 17 if (button1)
PRG 0:0af5376a3506 18 {
PRG 0:0af5376a3506 19 // Clockwise rotation
PRG 0:0af5376a3506 20 referenceVelocity = potMeterIn * maxVelocity;
PRG 0:0af5376a3506 21 }
PRG 0:0af5376a3506 22 else
PRG 0:0af5376a3506 23 {
PRG 0:0af5376a3506 24 // Counterclockwise rotation
PRG 0:0af5376a3506 25 referenceVelocity = -1*potMeterIn * maxVelocity;
PRG 0:0af5376a3506 26 }
PRG 0:0af5376a3506 27 return referenceVelocity;
PRG 0:0af5376a3506 28 }
PRG 0:0af5376a3506 29
PRG 0:0af5376a3506 30
PRG 0:0af5376a3506 31 void SetMotor1(float motorValue)
PRG 0:0af5376a3506 32 {
PRG 0:0af5376a3506 33 // Given -1<=motorValue<=1, this sets the PWM and direction
PRG 0:0af5376a3506 34 // bits for motor 1. Positive value makes motor rotating
PRG 0:0af5376a3506 35 // clockwise. motorValues outside range are truncated to
PRG 0:0af5376a3506 36 // within range
PRG 0:0af5376a3506 37 if (motorValue >=0) motor1DirectionPin=1;
PRG 0:0af5376a3506 38 else motor1DirectionPin=0;
PRG 0:0af5376a3506 39 if (fabs(motorValue)>1) motor1MagnitudePin = 1;
PRG 0:0af5376a3506 40 else motor1MagnitudePin = fabs(motorValue);
PRG 0:0af5376a3506 41
PRG 0:0af5376a3506 42 }
PRG 0:0af5376a3506 43 float FeedForwardControl(float referenceVelocity)
PRG 0:0af5376a3506 44 {
PRG 0:0af5376a3506 45 // very simple linear feed-forward control
PRG 0:0af5376a3506 46 const float MotorGain=8.4; // unit: (rad/s) / PWM
PRG 0:0af5376a3506 47 float motorValue = referenceVelocity / MotorGain;
PRG 0:0af5376a3506 48 return motorValue;
PRG 0:0af5376a3506 49 }
PRG 0:0af5376a3506 50
PRG 0:0af5376a3506 51 void MeasureAndControl(void)
PRG 0:0af5376a3506 52 {
PRG 0:0af5376a3506 53 // This function measures the potmeter position, extracts a
PRG 0:0af5376a3506 54 // reference velocity from it, and controls the motor with
PRG 0:0af5376a3506 55 // a simple FeedForward controller. Call this from a Ticker.
PRG 0:0af5376a3506 56 float referenceVelocity = GetReferenceVelocity();
PRG 0:0af5376a3506 57 float motorValue = FeedForwardControl(referenceVelocity);
PRG 0:0af5376a3506 58 SetMotor1(motorValue);
PRG 0:0af5376a3506 59 }
PRG 0:0af5376a3506 60
PRG 0:0af5376a3506 61 int main()
PRG 0:0af5376a3506 62 {
PRG 0:0af5376a3506 63 while (true)
PRG 0:0af5376a3506 64 {
PRG 0:0af5376a3506 65 GetReferenceVelocity();
PRG 0:0af5376a3506 66 FeedForwardControl(referenceVelocity);
PRG 0:0af5376a3506 67 MeasureAndControl();
PRG 0:0af5376a3506 68 printf("%f", referenceVelocity);
PRG 0:0af5376a3506 69
PRG 0:0af5376a3506 70 }
PRG 0:0af5376a3506 71 }