Arnoud Meutstege / Mbed 2 deprecated Tut5_motor_controller

first itteration

Dependencies:   MODSERIAL QEI mbed

main.cpp@2:abd40da4a5e2, 2017-10-04 (annotated)

Committer:
Arnoud113
Date:
Wed Oct 04 15:02:08 2017 +0000
Revision:
2:abd40da4a5e2
Parent:
1:659489c32e75
Child:
3:cc3766838777
Works as supposed to, problem was that I used DigitalOut where I should have used PwmOut, lesson learned. Only problem now is that after a certain amount of time the print function stops working. rest keeps woring as intended though

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