Roy van Zijl / Mbed 2 deprecated feed_forward_PI_copy

Werkend voor onze motor

Dependencies:   Encoder MODSERIAL QEI mbed

Fork of feed_forward_PI_copy by Dion de Greef

main.cpp@0:331597250051, 2017-10-03 (annotated)

Committer:
RoyvZ
Date:
Tue Oct 03 15:30:08 2017 +0000
Revision:
0:331597250051
Child:
1:92a60278860a
kijk naar v_ref waarom hij uit en aan gaat ipv harder en zachter

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RoyvZ 0:331597250051 1 #include "mbed.h"
RoyvZ 0:331597250051 2 #include "MODSERIAL.h"
RoyvZ 0:331597250051 3 #include "math.h"
RoyvZ 0:331597250051 4
RoyvZ 0:331597250051 5 DigitalOut gpo(D0);
RoyvZ 0:331597250051 6 DigitalOut motorDirection(D4);
RoyvZ 0:331597250051 7 DigitalOut motorSpeed(D5);
RoyvZ 0:331597250051 8 AnalogIn potMeterIn(A1);
RoyvZ 0:331597250051 9 InterruptIn button1(D3);
RoyvZ 0:331597250051 10 Ticker ticker;
RoyvZ 0:331597250051 11
RoyvZ 0:331597250051 12 MODSERIAL pc(USBTX, USBRX);
RoyvZ 0:331597250051 13
RoyvZ 0:331597250051 14
RoyvZ 0:331597250051 15 float GetReferenceVelocity()
RoyvZ 0:331597250051 16 {
RoyvZ 0:331597250051 17 // Returns reference velocity in rad/s.
RoyvZ 0:331597250051 18 // Positive value means clockwise rotation.
RoyvZ 0:331597250051 19 const float maxVelocity=8.4; // in rad/s of course!
RoyvZ 0:331597250051 20 float referenceVelocity; // in rad/s
RoyvZ 0:331597250051 21 if (button1) {
RoyvZ 0:331597250051 22 // Clockwise rotation
RoyvZ 0:331597250051 23 referenceVelocity = potMeterIn * maxVelocity;
RoyvZ 0:331597250051 24 }
RoyvZ 0:331597250051 25 else {
RoyvZ 0:331597250051 26 // Counterclockwise rotation
RoyvZ 0:331597250051 27 referenceVelocity = -1*potMeterIn * maxVelocity;
RoyvZ 0:331597250051 28 }
RoyvZ 0:331597250051 29 return referenceVelocity;
RoyvZ 0:331597250051 30 }
RoyvZ 0:331597250051 31
RoyvZ 0:331597250051 32 void setMotor(float motorValue) {
RoyvZ 0:331597250051 33 if (motorValue >= 0)
RoyvZ 0:331597250051 34 {
RoyvZ 0:331597250051 35 //float motor1DirectionPin1 = 1;
RoyvZ 0:331597250051 36 motorDirection=1;
RoyvZ 0:331597250051 37 }
RoyvZ 0:331597250051 38 else
RoyvZ 0:331597250051 39 {
RoyvZ 0:331597250051 40 //float motor1DirectionPin1 = 0;
RoyvZ 0:331597250051 41 motorDirection=0;
RoyvZ 0:331597250051 42 }
RoyvZ 0:331597250051 43
RoyvZ 0:331597250051 44 if (fabs(motorValue)>1)
RoyvZ 0:331597250051 45 {
RoyvZ 0:331597250051 46 //float motor1MagnitudePin1 = 1;
RoyvZ 0:331597250051 47 motorSpeed = 1;
RoyvZ 0:331597250051 48 }
RoyvZ 0:331597250051 49 else
RoyvZ 0:331597250051 50 {
RoyvZ 0:331597250051 51 //float motor1MagnitudePin1 = fabs(motorValue);
RoyvZ 0:331597250051 52 motorSpeed = fabs(motorValue);
RoyvZ 0:331597250051 53 }
RoyvZ 0:331597250051 54 }
RoyvZ 0:331597250051 55
RoyvZ 0:331597250051 56 float FeedForwardControl(float referenceVelocity)
RoyvZ 0:331597250051 57 {
RoyvZ 0:331597250051 58 // very simple linear feed-forward control
RoyvZ 0:331597250051 59 const float MotorGain=8.4; // unit: (rad/s) / PWM
RoyvZ 0:331597250051 60 float motorValue = referenceVelocity / MotorGain;
RoyvZ 0:331597250051 61 return motorValue;
RoyvZ 0:331597250051 62 }
RoyvZ 0:331597250051 63
RoyvZ 0:331597250051 64
RoyvZ 0:331597250051 65 void MeasureAndControl(void)
RoyvZ 0:331597250051 66 {
RoyvZ 0:331597250051 67 // This function measures the potmeter position, extracts a
RoyvZ 0:331597250051 68 // reference velocity from it, and controls the motor with
RoyvZ 0:331597250051 69 // a simple FeedForward controller. Call this from a Ticker.
RoyvZ 0:331597250051 70 float referenceVelocity = GetReferenceVelocity();
RoyvZ 0:331597250051 71 float motorValue = FeedForwardControl(referenceVelocity);
RoyvZ 0:331597250051 72 setMotor(motorValue);
RoyvZ 0:331597250051 73 }
RoyvZ 0:331597250051 74
RoyvZ 0:331597250051 75 int main() {
RoyvZ 0:331597250051 76 pc.baud(115200);
RoyvZ 0:331597250051 77 //ticker.attach(MeasureAndControl, 0.01);
RoyvZ 0:331597250051 78 while(true){
RoyvZ 0:331597250051 79 wait(0.1);
RoyvZ 0:331597250051 80
RoyvZ 0:331597250051 81 //pc.printf("%f\r\n",GetReferenceVelocity());
RoyvZ 0:331597250051 82 float v_ref = GetReferenceVelocity();
RoyvZ 0:331597250051 83 setMotor(v_ref);
RoyvZ 0:331597250051 84 pc.printf("%f \r\n", FeedForwardControl(v_ref));
RoyvZ 0:331597250051 85 motorDirection.write(motorDirection);
RoyvZ 0:331597250051 86 motorSpeed.write(motorSpeed); //PWM Speed Control
RoyvZ 0:331597250051 87 }
RoyvZ 0:331597250051 88 }
RoyvZ 0:331597250051 89