V D / Mbed 2 deprecated 20171019_Feedforwardcontroller_referencevelocity

Feedforward controller

Dependencies:   MODSERIAL QEI mbed

Fork of Tut5_motor_controller by Arnoud Meutstege

main.cpp@3:cc3766838777, 2017-10-05 (annotated)

Committer:
Arnoud113
Date:
Thu Oct 05 13:10:39 2017 +0000
Revision:
3:cc3766838777
Parent:
2:abd40da4a5e2
Child:
4:983b50758735
20171005_goed werkende versie opracht 1

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 3:cc3766838777 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 3:cc3766838777 23 Ticker controller;
Arnoud113 1:659489c32e75 24
Arnoud113 0:1816743ba8ba 25
Arnoud113 0:1816743ba8ba 26 float GetReferenceVelocity()
Arnoud113 0:1816743ba8ba 27 {
Arnoud113 0:1816743ba8ba 28 // Returns reference velocity in rad/s.
Arnoud113 0:1816743ba8ba 29 // Positive value means clockwise rotation.
Arnoud113 0:1816743ba8ba 30
Arnoud113 0:1816743ba8ba 31 const float maxVelocity=8.4; // in rad/s of course!
Arnoud113 0:1816743ba8ba 32
Arnoud113 0:1816743ba8ba 33 float referenceVelocity; // in rad/s
Arnoud113 0:1816743ba8ba 34
Arnoud113 0:1816743ba8ba 35 if (button1) {
Arnoud113 0:1816743ba8ba 36 // Clockwise rotation
Arnoud113 3:cc3766838777 37 referenceVelocity = potMeterIn1 * maxVelocity;
Arnoud113 3:cc3766838777 38 ledr = 1;
Arnoud113 3:cc3766838777 39 ledb = 0;
Arnoud113 0:1816743ba8ba 40 }
Arnoud113 0:1816743ba8ba 41 else {
Arnoud113 0:1816743ba8ba 42 // Counterclockwise rotation
Arnoud113 3:cc3766838777 43 referenceVelocity = -1*potMeterIn1 * maxVelocity;
Arnoud113 3:cc3766838777 44 ledb = 1;
Arnoud113 3:cc3766838777 45 ledr = 0;
Arnoud113 0:1816743ba8ba 46 }
Arnoud113 0:1816743ba8ba 47 return referenceVelocity;
Arnoud113 0:1816743ba8ba 48 }
Arnoud113 0:1816743ba8ba 49
Arnoud113 0:1816743ba8ba 50
Arnoud113 0:1816743ba8ba 51 float FeedForwardControl(float &referenceVelocity)
Arnoud113 0:1816743ba8ba 52 {
Arnoud113 0:1816743ba8ba 53 // very simple linear feed-forward control
Arnoud113 0:1816743ba8ba 54 const float MotorGain=8.4; // unit: (rad/s) / PWM
Arnoud113 0:1816743ba8ba 55 float motorValue = referenceVelocity / MotorGain;
Arnoud113 0:1816743ba8ba 56 return motorValue;
Arnoud113 0:1816743ba8ba 57 }
Arnoud113 0:1816743ba8ba 58
Arnoud113 0:1816743ba8ba 59 void SetMotor1(float motorValue)
Arnoud113 0:1816743ba8ba 60 {
Arnoud113 0:1816743ba8ba 61 // Given -1<=motorValue<=1, this sets the PWM and direction
Arnoud113 0:1816743ba8ba 62 // bits for motor 1. Positive value makes motor rotating
Arnoud113 0:1816743ba8ba 63 // clockwise. motorValues outside range are truncated to
Arnoud113 0:1816743ba8ba 64 // within range
Arnoud113 1:659489c32e75 65 if (motorValue >=0) motor1DC= 1;
Arnoud113 0:1816743ba8ba 66 else motor1DC=0;
Arnoud113 0:1816743ba8ba 67 if (fabs(motorValue)>1) motor1PWM = 1;
Arnoud113 0:1816743ba8ba 68 else motor1PWM = fabs(motorValue);
Arnoud113 0:1816743ba8ba 69 }
Arnoud113 0:1816743ba8ba 70
Arnoud113 1:659489c32e75 71 void MeasureAndControl(void)
Arnoud113 1:659489c32e75 72 {
Arnoud113 1:659489c32e75 73 float referenceVelocity = GetReferenceVelocity();
Arnoud113 1:659489c32e75 74 float motorValue = FeedForwardControl(referenceVelocity);
Arnoud113 2:abd40da4a5e2 75 SetMotor1(motorValue);
Arnoud113 1:659489c32e75 76 }
Arnoud113 1:659489c32e75 77
Arnoud113 3:cc3766838777 78
Arnoud113 3:cc3766838777 79
Arnoud113 0:1816743ba8ba 80 int main()
Arnoud113 0:1816743ba8ba 81 {
Arnoud113 0:1816743ba8ba 82 pc.baud(115200);
Arnoud113 2:abd40da4a5e2 83 QEI Encoder(D12,D13,NC,32);
Arnoud113 3:cc3766838777 84
Arnoud113 3:cc3766838777 85 ledr = 1;
Arnoud113 3:cc3766838777 86 ledb = 1;
Arnoud113 3:cc3766838777 87 ledg = 1;
Arnoud113 0:1816743ba8ba 88
Arnoud113 2:abd40da4a5e2 89 controller.attach(&MeasureAndControl, 0.1);
Arnoud113 3:cc3766838777 90
Arnoud113 1:659489c32e75 91 while(1)
Arnoud113 3:cc3766838777 92 {
Arnoud113 2:abd40da4a5e2 93 int counts = Encoder.getPulses();
Arnoud113 2:abd40da4a5e2 94 float rV = GetReferenceVelocity();
Arnoud113 2:abd40da4a5e2 95 float mV = FeedForwardControl(rV);
Arnoud113 2:abd40da4a5e2 96
Arnoud113 2:abd40da4a5e2 97 pc.printf("\r reference velocity: %f. Motor Value is: %f number of counts: %i\n",mV,rV,counts);
Arnoud113 2:abd40da4a5e2 98 }
Arnoud113 2:abd40da4a5e2 99
Arnoud113 0:1816743ba8ba 100 }
Arnoud113 0:1816743ba8ba 101
Arnoud113 0:1816743ba8ba 102