V D / Mbed 2 deprecated 20171019_Feedforwardcontroller_referencevelocity

Feedforward controller

Dependencies:   MODSERIAL QEI mbed

Fork of Tut5_motor_controller by Arnoud Meutstege

main.cpp@4:983b50758735, 2017-10-06 (annotated)

Committer:
Arnoud113
Date:
Fri Oct 06 09:05:35 2017 +0000
Revision:
4:983b50758735
Parent:
3:cc3766838777
Child:
5:6c16e9335262
shde;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Arnoud113 0:1816743ba8ba 1 #include "mbed.h"
Arnoud113 0:1816743ba8ba 2 #include "QEI.h"
Arnoud113 0:1816743ba8ba 3 #include "MODSERIAL.h"
Arnoud113 0:1816743ba8ba 4 #include "math.h"
Arnoud113 0:1816743ba8ba 5
Arnoud113 0:1816743ba8ba 6
Arnoud113 0:1816743ba8ba 7
Arnoud113 0:1816743ba8ba 8 DigitalOut gpo(D0);
Arnoud113 3:cc3766838777 9 DigitalOut ledb(LED_BLUE);
Arnoud113 3:cc3766838777 10 DigitalOut ledr(LED_RED);
Arnoud113 3:cc3766838777 11 DigitalOut ledg(LED_GREEN);
Arnoud113 1:659489c32e75 12 DigitalOut motor1DC(D7);
Arnoud113 2:abd40da4a5e2 13 PwmOut motor1PWM(D6);
Arnoud113 0:1816743ba8ba 14 DigitalOut motor2DC(D4);
Arnoud113 2:abd40da4a5e2 15 PwmOut motor2PWM(D5);
Arnoud113 0:1816743ba8ba 16
Arnoud113 3:cc3766838777 17 AnalogIn potMeterIn1(A0);
Arnoud113 4:983b50758735 18 AnalogIn potMeterIn2(A1);
Arnoud113 3:cc3766838777 19
Arnoud113 0:1816743ba8ba 20 DigitalIn button1(D11);
Arnoud113 0:1816743ba8ba 21
Arnoud113 0:1816743ba8ba 22 MODSERIAL pc(USBTX,USBRX);
Arnoud113 4:983b50758735 23 QEI Encoder(D12,D13,NC,32);
Arnoud113 3:cc3766838777 24 Ticker controller;
Arnoud113 1:659489c32e75 25
Arnoud113 0:1816743ba8ba 26
Arnoud113 0:1816743ba8ba 27 float GetReferenceVelocity()
Arnoud113 0:1816743ba8ba 28 {
Arnoud113 0:1816743ba8ba 29 // Returns reference velocity in rad/s.
Arnoud113 0:1816743ba8ba 30 // Positive value means clockwise rotation.
Arnoud113 0:1816743ba8ba 31
Arnoud113 0:1816743ba8ba 32 const float maxVelocity=8.4; // in rad/s of course!
Arnoud113 0:1816743ba8ba 33
Arnoud113 0:1816743ba8ba 34 float referenceVelocity; // in rad/s
Arnoud113 0:1816743ba8ba 35
Arnoud113 0:1816743ba8ba 36 if (button1) {
Arnoud113 0:1816743ba8ba 37 // Clockwise rotation
Arnoud113 3:cc3766838777 38 referenceVelocity = potMeterIn1 * maxVelocity;
Arnoud113 3:cc3766838777 39 ledr = 1;
Arnoud113 3:cc3766838777 40 ledb = 0;
Arnoud113 0:1816743ba8ba 41 }
Arnoud113 0:1816743ba8ba 42 else {
Arnoud113 0:1816743ba8ba 43 // Counterclockwise rotation
Arnoud113 3:cc3766838777 44 referenceVelocity = -1*potMeterIn1 * maxVelocity;
Arnoud113 3:cc3766838777 45 ledb = 1;
Arnoud113 3:cc3766838777 46 ledr = 0;
Arnoud113 0:1816743ba8ba 47 }
Arnoud113 0:1816743ba8ba 48 return referenceVelocity;
Arnoud113 0:1816743ba8ba 49 }
Arnoud113 0:1816743ba8ba 50
Arnoud113 0:1816743ba8ba 51
Arnoud113 0:1816743ba8ba 52 float FeedForwardControl(float &referenceVelocity)
Arnoud113 0:1816743ba8ba 53 {
Arnoud113 0:1816743ba8ba 54 // very simple linear feed-forward control
Arnoud113 0:1816743ba8ba 55 const float MotorGain=8.4; // unit: (rad/s) / PWM
Arnoud113 0:1816743ba8ba 56 float motorValue = referenceVelocity / MotorGain;
Arnoud113 0:1816743ba8ba 57 return motorValue;
Arnoud113 0:1816743ba8ba 58 }
Arnoud113 0:1816743ba8ba 59
Arnoud113 4:983b50758735 60 float GetReferencePosition()
Arnoud113 4:983b50758735 61 {
Arnoud113 4:983b50758735 62 int potmultiplier = 4200; // constant to multiply the pot 2 value with to get a reference position
Arnoud113 4:983b50758735 63
Arnoud113 4:983b50758735 64 float referencePosition;
Arnoud113 4:983b50758735 65 referencePosition = potMeterIn2 * potmultiplier;
Arnoud113 4:983b50758735 66 return referencePosition;
Arnoud113 4:983b50758735 67 }
Arnoud113 4:983b50758735 68
Arnoud113 0:1816743ba8ba 69 void SetMotor1(float motorValue)
Arnoud113 0:1816743ba8ba 70 {
Arnoud113 0:1816743ba8ba 71 // Given -1<=motorValue<=1, this sets the PWM and direction
Arnoud113 0:1816743ba8ba 72 // bits for motor 1. Positive value makes motor rotating
Arnoud113 0:1816743ba8ba 73 // clockwise. motorValues outside range are truncated to
Arnoud113 0:1816743ba8ba 74 // within range
Arnoud113 1:659489c32e75 75 if (motorValue >=0) motor1DC= 1;
Arnoud113 0:1816743ba8ba 76 else motor1DC=0;
Arnoud113 0:1816743ba8ba 77 if (fabs(motorValue)>1) motor1PWM = 1;
Arnoud113 0:1816743ba8ba 78 else motor1PWM = fabs(motorValue);
Arnoud113 0:1816743ba8ba 79 }
Arnoud113 0:1816743ba8ba 80
Arnoud113 1:659489c32e75 81 void MeasureAndControl(void)
Arnoud113 1:659489c32e75 82 {
Arnoud113 1:659489c32e75 83 float referenceVelocity = GetReferenceVelocity();
Arnoud113 4:983b50758735 84 int referencePosition = GetReferencePosition();
Arnoud113 4:983b50758735 85 int counts = Encoder.getPulses();
Arnoud113 4:983b50758735 86
Arnoud113 4:983b50758735 87
Arnoud113 4:983b50758735 88
Arnoud113 4:983b50758735 89 if(counts < referencePosition)
Arnoud113 4:983b50758735 90 {
Arnoud113 4:983b50758735 91 float motorValue = FeedForwardControl(referenceVelocity);
Arnoud113 4:983b50758735 92 SetMotor1(motorValue);
Arnoud113 4:983b50758735 93 }
Arnoud113 4:983b50758735 94 else
Arnoud113 4:983b50758735 95 {
Arnoud113 4:983b50758735 96 int b = -1;
Arnoud113 4:983b50758735 97 float motorValue = b * FeedForwardControl(referenceVelocity);
Arnoud113 4:983b50758735 98 SetMotor1(motorValue);
Arnoud113 4:983b50758735 99 }
Arnoud113 4:983b50758735 100
Arnoud113 1:659489c32e75 101 }
Arnoud113 1:659489c32e75 102
Arnoud113 3:cc3766838777 103
Arnoud113 3:cc3766838777 104
Arnoud113 0:1816743ba8ba 105 int main()
Arnoud113 0:1816743ba8ba 106 {
Arnoud113 0:1816743ba8ba 107 pc.baud(115200);
Arnoud113 2:abd40da4a5e2 108 QEI Encoder(D12,D13,NC,32);
Arnoud113 3:cc3766838777 109
Arnoud113 3:cc3766838777 110 ledr = 1;
Arnoud113 3:cc3766838777 111 ledb = 1;
Arnoud113 3:cc3766838777 112 ledg = 1;
Arnoud113 0:1816743ba8ba 113
Arnoud113 2:abd40da4a5e2 114 controller.attach(&MeasureAndControl, 0.1);
Arnoud113 3:cc3766838777 115
Arnoud113 1:659489c32e75 116 while(1)
Arnoud113 3:cc3766838777 117 {
Arnoud113 2:abd40da4a5e2 118 int counts = Encoder.getPulses();
Arnoud113 2:abd40da4a5e2 119 float rV = GetReferenceVelocity();
Arnoud113 2:abd40da4a5e2 120 float mV = FeedForwardControl(rV);
Arnoud113 4:983b50758735 121 int rP = GetReferencePosition();
Arnoud113 2:abd40da4a5e2 122
Arnoud113 4:983b50758735 123 pc.printf("\r reference velocity: %f. Reference Position: %i Motor Value is: %f number of counts: %i\n",mV,rP,rV,counts);
Arnoud113 2:abd40da4a5e2 124 }
Arnoud113 2:abd40da4a5e2 125
Arnoud113 0:1816743ba8ba 126 }
Arnoud113 0:1816743ba8ba 127
Arnoud113 0:1816743ba8ba 128