V D / Mbed 2 deprecated 20171019_Feedforwardcontroller_referencevelocity

Feedforward controller

Dependencies:   MODSERIAL QEI mbed

Fork of Tut5_motor_controller by Arnoud Meutstege

main.cpp@5:6c16e9335262, 2017-10-19 (annotated)

Committer:
vd
Date:
Thu Oct 19 12:11:53 2017 +0000
Revision:
5:6c16e9335262
Parent:
4:983b50758735 
Feedforward controller, set reference velocity;

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 DigitalOut gpo(D0);
Arnoud113 3:cc3766838777 7 DigitalOut ledb(LED_BLUE);
Arnoud113 3:cc3766838777 8 DigitalOut ledr(LED_RED);
Arnoud113 3:cc3766838777 9 DigitalOut ledg(LED_GREEN);
vd 5:6c16e9335262 10
vd 5:6c16e9335262 11 DigitalOut motor1DC(D7);
vd 5:6c16e9335262 12 PwmOut motor1PWM(D6);
Arnoud113 0:1816743ba8ba 13 DigitalOut motor2DC(D4);
Arnoud113 2:abd40da4a5e2 14 PwmOut motor2PWM(D5);
Arnoud113 0:1816743ba8ba 15
vd 5:6c16e9335262 16 AnalogIn potMeterIn1(A0);
vd 5:6c16e9335262 17 AnalogIn potMeterIn2(A1);
Arnoud113 3:cc3766838777 18
Arnoud113 0:1816743ba8ba 19 DigitalIn button1(D11);
vd 5:6c16e9335262 20 DigitalIn button2(D10);
Arnoud113 0:1816743ba8ba 21
Arnoud113 0:1816743ba8ba 22 MODSERIAL pc(USBTX,USBRX);
Arnoud113 4:983b50758735 23 QEI Encoder(D12,D13,NC,32);
vd 5:6c16e9335262 24 Ticker controller; // ticker with the name 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
vd 5:6c16e9335262 36
Arnoud113 0:1816743ba8ba 37 if (button1) {
vd 5:6c16e9335262 38 // Clockwise rotation // this should be counterclockwise (what is chosen then: CCW = positive direction)
vd 5:6c16e9335262 39 // als button niet is ingedrukt:
vd 5:6c16e9335262 40 // linksom = positief
vd 5:6c16e9335262 41
Arnoud113 3:cc3766838777 42 referenceVelocity = potMeterIn1 * maxVelocity;
Arnoud113 3:cc3766838777 43 ledr = 1;
Arnoud113 3:cc3766838777 44 ledb = 0;
Arnoud113 0:1816743ba8ba 45 }
Arnoud113 0:1816743ba8ba 46 else {
vd 5:6c16e9335262 47 // Counterclockwise rotation // this should be clockwise
vd 5:6c16e9335262 48 // als button wel is ingedrukt:
Arnoud113 3:cc3766838777 49 referenceVelocity = -1*potMeterIn1 * maxVelocity;
Arnoud113 3:cc3766838777 50 ledb = 1;
Arnoud113 3:cc3766838777 51 ledr = 0;
Arnoud113 0:1816743ba8ba 52 }
vd 5:6c16e9335262 53
vd 5:6c16e9335262 54
vd 5:6c16e9335262 55 /*
vd 5:6c16e9335262 56
vd 5:6c16e9335262 57 int rP1 = GetReferencePosition();
vd 5:6c16e9335262 58 int counts = Encoder.getPulses();
vd 5:6c16e9335262 59
vd 5:6c16e9335262 60 if (counts > rP) {
vd 5:6c16e9335262 61
vd 5:6c16e9335262 62 referenceVelocity = 0*potMeterIn1 * maxVelocity;;
vd 5:6c16e9335262 63 ledb = 1;
vd 5:6c16e9335262 64 ledr = 0;
vd 5:6c16e9335262 65 }
vd 5:6c16e9335262 66
vd 5:6c16e9335262 67 */
vd 5:6c16e9335262 68
vd 5:6c16e9335262 69
vd 5:6c16e9335262 70 /*
vd 5:6c16e9335262 71
vd 5:6c16e9335262 72 if (button2==false) {
vd 5:6c16e9335262 73
vd 5:6c16e9335262 74 referenceVelocity = 0*potMeterIn1 * maxVelocity;;
vd 5:6c16e9335262 75 ledb = 1;
vd 5:6c16e9335262 76 ledr = 0;
vd 5:6c16e9335262 77
vd 5:6c16e9335262 78 }
vd 5:6c16e9335262 79 */
vd 5:6c16e9335262 80
vd 5:6c16e9335262 81 // check of hij stopt
vd 5:6c16e9335262 82
vd 5:6c16e9335262 83
vd 5:6c16e9335262 84
Arnoud113 0:1816743ba8ba 85 return referenceVelocity;
Arnoud113 0:1816743ba8ba 86 }
Arnoud113 0:1816743ba8ba 87
Arnoud113 0:1816743ba8ba 88
vd 5:6c16e9335262 89 float FeedForwardControl(float &referenceVelocity) // is de motorvalue
Arnoud113 0:1816743ba8ba 90 {
Arnoud113 0:1816743ba8ba 91 // very simple linear feed-forward control
Arnoud113 0:1816743ba8ba 92 const float MotorGain=8.4; // unit: (rad/s) / PWM
Arnoud113 0:1816743ba8ba 93 float motorValue = referenceVelocity / MotorGain;
Arnoud113 0:1816743ba8ba 94 return motorValue;
Arnoud113 0:1816743ba8ba 95 }
Arnoud113 0:1816743ba8ba 96
Arnoud113 4:983b50758735 97 float GetReferencePosition()
Arnoud113 4:983b50758735 98 {
vd 5:6c16e9335262 99 int potmultiplier = 4200; // constant to multiply the pot 2 value with to get a reference position
vd 5:6c16e9335262 100 // integrated quadrature encoder that provides a resolution of 64 counts per revolution of the motor shaft, which corresponds to 8400 counts per revolution of the gearbox’s output shaft.
vd 5:6c16e9335262 101 int referencePosition;
vd 5:6c16e9335262 102 referencePosition = (potMeterIn2 * potmultiplier);
Arnoud113 4:983b50758735 103 return referencePosition;
Arnoud113 4:983b50758735 104 }
Arnoud113 4:983b50758735 105
Arnoud113 0:1816743ba8ba 106 void SetMotor1(float motorValue)
Arnoud113 0:1816743ba8ba 107 {
Arnoud113 0:1816743ba8ba 108 // Given -1<=motorValue<=1, this sets the PWM and direction
Arnoud113 0:1816743ba8ba 109 // bits for motor 1. Positive value makes motor rotating
Arnoud113 0:1816743ba8ba 110 // clockwise. motorValues outside range are truncated to
Arnoud113 0:1816743ba8ba 111 // within range
vd 5:6c16e9335262 112 if (motorValue >=0) motor1DC= 1; // direction
Arnoud113 0:1816743ba8ba 113 else motor1DC=0;
vd 5:6c16e9335262 114 if (fabs(motorValue)>1) motor1PWM = 1; //??. motor1PWM is the speed
vd 5:6c16e9335262 115 else motor1PWM = fabs(motorValue);
Arnoud113 0:1816743ba8ba 116 }
Arnoud113 0:1816743ba8ba 117
Arnoud113 1:659489c32e75 118 void MeasureAndControl(void)
Arnoud113 1:659489c32e75 119 {
vd 5:6c16e9335262 120
vd 5:6c16e9335262 121 float referenceVelocity = GetReferenceVelocity(); // these variables are going to be ticked by 0.1
Arnoud113 4:983b50758735 122 int referencePosition = GetReferencePosition();
Arnoud113 4:983b50758735 123 int counts = Encoder.getPulses();
Arnoud113 4:983b50758735 124
vd 5:6c16e9335262 125
Arnoud113 4:983b50758735 126
vd 5:6c16e9335262 127 // if(counts < referencePosition)
vd 5:6c16e9335262 128
vd 5:6c16e9335262 129
Arnoud113 4:983b50758735 130 {
Arnoud113 4:983b50758735 131 float motorValue = FeedForwardControl(referenceVelocity);
Arnoud113 4:983b50758735 132 SetMotor1(motorValue);
Arnoud113 4:983b50758735 133 }
vd 5:6c16e9335262 134
vd 5:6c16e9335262 135
vd 5:6c16e9335262 136 /*
vd 5:6c16e9335262 137 if(button2==false)
vd 5:6c16e9335262 138 {
vd 5:6c16e9335262 139 float motorValue = -1 * FeedForwardControl(referenceVelocity);
vd 5:6c16e9335262 140 SetMotor1(motorValue);
vd 5:6c16e9335262 141 }
vd 5:6c16e9335262 142
vd 5:6c16e9335262 143 if(referencePosition==1575) // dit werkt!
vd 5:6c16e9335262 144 {
vd 5:6c16e9335262 145 float motorValue = -1 * FeedForwardControl(referenceVelocity);
vd 5:6c16e9335262 146 SetMotor1(motorValue);
vd 5:6c16e9335262 147 }
vd 5:6c16e9335262 148
vd 5:6c16e9335262 149 if(referencePosition > 1575) // dit werkt!
vd 5:6c16e9335262 150 {
vd 5:6c16e9335262 151 float motorValue = -1 * FeedForwardControl(referenceVelocity);
vd 5:6c16e9335262 152 SetMotor1(motorValue);
vd 5:6c16e9335262 153 }
vd 5:6c16e9335262 154
vd 5:6c16e9335262 155 */
vd 5:6c16e9335262 156
vd 5:6c16e9335262 157 if(counts > 1575) // dit werkt niet...
vd 5:6c16e9335262 158 {
vd 5:6c16e9335262 159 float motorValue = -1 * FeedForwardControl(referenceVelocity);
vd 5:6c16e9335262 160 SetMotor1(motorValue);
vd 5:6c16e9335262 161 }
vd 5:6c16e9335262 162
vd 5:6c16e9335262 163
vd 5:6c16e9335262 164
vd 5:6c16e9335262 165
vd 5:6c16e9335262 166 /*
Arnoud113 4:983b50758735 167 else
Arnoud113 4:983b50758735 168 {
vd 5:6c16e9335262 169 float motorValue = -1 * FeedForwardControl(referenceVelocity);
Arnoud113 4:983b50758735 170 SetMotor1(motorValue);
vd 5:6c16e9335262 171 }
vd 5:6c16e9335262 172 */
vd 5:6c16e9335262 173
Arnoud113 4:983b50758735 174
Arnoud113 1:659489c32e75 175 }
Arnoud113 1:659489c32e75 176
Arnoud113 3:cc3766838777 177
Arnoud113 3:cc3766838777 178
Arnoud113 0:1816743ba8ba 179 int main()
Arnoud113 0:1816743ba8ba 180 {
vd 5:6c16e9335262 181 pc.baud(9600);
Arnoud113 2:abd40da4a5e2 182 QEI Encoder(D12,D13,NC,32);
Arnoud113 3:cc3766838777 183
Arnoud113 3:cc3766838777 184 ledr = 1;
Arnoud113 3:cc3766838777 185 ledb = 1;
Arnoud113 3:cc3766838777 186 ledg = 1;
Arnoud113 0:1816743ba8ba 187
vd 5:6c16e9335262 188 controller.attach(&MeasureAndControl, 0.1); //ticker. Change ticker, what happens? maybe then conditions with counts work?
vd 5:6c16e9335262 189 //controller.attach(&MeasureAndControl, 1); // werkt niet
vd 5:6c16e9335262 190 //controller.attach(&MeasureAndControl, 0.01); // werkt niet
vd 5:6c16e9335262 191 //controller.attach(&MeasureAndControl, 0.1); // origineel
vd 5:6c16e9335262 192
Arnoud113 3:cc3766838777 193
Arnoud113 1:659489c32e75 194 while(1)
Arnoud113 3:cc3766838777 195 {
vd 5:6c16e9335262 196
vd 5:6c16e9335262 197 float rV = GetReferenceVelocity();
Arnoud113 4:983b50758735 198 int rP = GetReferencePosition();
vd 5:6c16e9335262 199 float mV = FeedForwardControl(rV);
vd 5:6c16e9335262 200 int counts = Encoder.getPulses();
vd 5:6c16e9335262 201
vd 5:6c16e9335262 202
vd 5:6c16e9335262 203 /*
vd 5:6c16e9335262 204 if(counts > rP) // dit werkt niet... in de main, while loop
vd 5:6c16e9335262 205
vd 5:6c16e9335262 206 {
vd 5:6c16e9335262 207 float motorValue = 0 * FeedForwardControl(rV);
vd 5:6c16e9335262 208 SetMotor1(motorValue); // hij verandert niet van richting, maar beweegt heen en weer
vd 5:6c16e9335262 209 }
vd 5:6c16e9335262 210
vd 5:6c16e9335262 211 */
vd 5:6c16e9335262 212
vd 5:6c16e9335262 213 if(button2 == false) // counts > rP &&
vd 5:6c16e9335262 214
vd 5:6c16e9335262 215 {
vd 5:6c16e9335262 216 const float maxVelocity=8.4; // in rad/s of course!
vd 5:6c16e9335262 217 rV = 0*potMeterIn1 * maxVelocity;;
vd 5:6c16e9335262 218 ledb = 1;
vd 5:6c16e9335262 219 ledr = 0;
vd 5:6c16e9335262 220
Arnoud113 2:abd40da4a5e2 221
vd 5:6c16e9335262 222 float motorValue = 0 * FeedForwardControl(rV);
vd 5:6c16e9335262 223 SetMotor1(motorValue); // hij verandert niet van richting, maar beweegt heen en weer
vd 5:6c16e9335262 224
vd 5:6c16e9335262 225 }
vd 5:6c16e9335262 226
vd 5:6c16e9335262 227
vd 5:6c16e9335262 228
vd 5:6c16e9335262 229
vd 5:6c16e9335262 230 wait(0.2) ;
vd 5:6c16e9335262 231 pc.printf("\r reference velocity: %0.2f. Reference Position: %i Motor Value is: %0.2f number of counts: %i\n",rV,rP,mV,counts);
vd 5:6c16e9335262 232
Arnoud113 2:abd40da4a5e2 233 }
vd 5:6c16e9335262 234
Arnoud113 2:abd40da4a5e2 235
Arnoud113 0:1816743ba8ba 236 }
Arnoud113 0:1816743ba8ba 237
vd 5:6c16e9335262 238 // Voor Serial gebruik: Mac Terminal: screen /dev/tty.usbmodem1422 9600
Arnoud113 0:1816743ba8ba 239