Arnoud Meutstege / Mbed 2 deprecated Tut5_motor_controller

first itteration

Dependencies:   MODSERIAL QEI mbed

main.cpp@1:659489c32e75, 2017-10-04 (annotated)

Committer:
Arnoud113
Date:
Wed Oct 04 14:15:39 2017 +0000
Revision:
1:659489c32e75
Parent:
0:1816743ba8ba
Child:
2:abd40da4a5e2
Rewrote script to specs of the assignment given in the powerpoint of Tutorial 5;

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 1:659489c32e75 11 DigitalOut motor1PWM(D6);
Arnoud113 0:1816743ba8ba 12 DigitalOut motor2DC(D4);
Arnoud113 0:1816743ba8ba 13 DigitalOut 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 1:659489c32e75 19 QEI Encoder(D12,D13,NC,32); // Input for the Encoder
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 1:659489c32e75 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 1:659489c32e75 74
Arnoud113 1:659489c32e75 75 //float v = GetReferenceVelocity();
Arnoud113 1:659489c32e75 76 //float controlValue = FeedForwardControl(v);
Arnoud113 1:659489c32e75 77 //SetMotor1(controlValue);
Arnoud113 0:1816743ba8ba 78
Arnoud113 1:659489c32e75 79 controller.attach(&MeasureAndControl,0.01);
Arnoud113 0:1816743ba8ba 80
Arnoud113 1:659489c32e75 81 int counts = Encoder.getPulses();
Arnoud113 1:659489c32e75 82 pc.printf("\r `control value: %f reference velocity: %f. Motor Value is: %f number of counts: %i\n",referenceVelocity ,motorValue,counts);
Arnoud113 1:659489c32e75 83
Arnoud113 1:659489c32e75 84 while(1)
Arnoud113 1:659489c32e75 85 {}
Arnoud113 0:1816743ba8ba 86 }
Arnoud113 0:1816743ba8ba 87
Arnoud113 0:1816743ba8ba 88