Arnoud Meutstege / Mbed 2 deprecated Tut5_motor_controller

first itteration

Dependencies:   MODSERIAL QEI mbed

main.cpp@7:080805fc3cf0, 2017-10-27 (annotated)

Committer:
Arnoud113
Date:
Fri Oct 27 10:22:59 2017 +0000
Revision:
7:080805fc3cf0
Parent:
6:1a410879a539 
WEG

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 5:a3848a66a4df 27
Arnoud113 5:a3848a66a4df 28 float GetReferencePosition()
Arnoud113 5:a3848a66a4df 29 {
Arnoud113 6:1a410879a539 30 int potmultiplier = 8000; // constant to multiply the pot 2 value with to get a reference position
Arnoud113 5:a3848a66a4df 31
Arnoud113 5:a3848a66a4df 32 float referencePosition;
Arnoud113 5:a3848a66a4df 33 referencePosition = potMeterIn2 * potmultiplier;
Arnoud113 5:a3848a66a4df 34 return referencePosition;
Arnoud113 5:a3848a66a4df 35 }
Arnoud113 5:a3848a66a4df 36
Arnoud113 0:1816743ba8ba 37 float GetReferenceVelocity()
Arnoud113 0:1816743ba8ba 38 {
Arnoud113 0:1816743ba8ba 39 // Returns reference velocity in rad/s.
Arnoud113 0:1816743ba8ba 40 // Positive value means clockwise rotation.
Arnoud113 0:1816743ba8ba 41
Arnoud113 6:1a410879a539 42 int referencePosition = GetReferencePosition();
Arnoud113 6:1a410879a539 43 int counts = Encoder.getPulses();
Arnoud113 6:1a410879a539 44
Arnoud113 0:1816743ba8ba 45 const float maxVelocity=8.4; // in rad/s of course!
Arnoud113 0:1816743ba8ba 46
Arnoud113 0:1816743ba8ba 47 float referenceVelocity; // in rad/s
Arnoud113 0:1816743ba8ba 48
Arnoud113 6:1a410879a539 49 if (counts > referencePosition) {
Arnoud113 0:1816743ba8ba 50 // Clockwise rotation
Arnoud113 6:1a410879a539 51 referenceVelocity = -1*potMeterIn1 * maxVelocity;
Arnoud113 3:cc3766838777 52 ledr = 1;
Arnoud113 6:1a410879a539 53 ledg = 1;
Arnoud113 3:cc3766838777 54 ledb = 0;
Arnoud113 0:1816743ba8ba 55 }
Arnoud113 6:1a410879a539 56 else if ( counts < referencePosition){
Arnoud113 0:1816743ba8ba 57 // Counterclockwise rotation
Arnoud113 6:1a410879a539 58 referenceVelocity = potMeterIn1 * maxVelocity;
Arnoud113 3:cc3766838777 59 ledb = 1;
Arnoud113 6:1a410879a539 60 ledg = 1;
Arnoud113 3:cc3766838777 61 ledr = 0;
Arnoud113 0:1816743ba8ba 62 }
Arnoud113 6:1a410879a539 63 else{
Arnoud113 6:1a410879a539 64 referenceVelocity = 0;
Arnoud113 6:1a410879a539 65 ledb = 1;
Arnoud113 6:1a410879a539 66 ledr = 1;
Arnoud113 6:1a410879a539 67 ledg = 0;
Arnoud113 6:1a410879a539 68 }
Arnoud113 0:1816743ba8ba 69 return referenceVelocity;
Arnoud113 0:1816743ba8ba 70 }
Arnoud113 0:1816743ba8ba 71
Arnoud113 6:1a410879a539 72 float FeedForwardControl(float &referenceVelocity)
Arnoud113 0:1816743ba8ba 73 {
Arnoud113 0:1816743ba8ba 74 // very simple linear feed-forward control
Arnoud113 0:1816743ba8ba 75 const float MotorGain=8.4; // unit: (rad/s) / PWM
Arnoud113 6:1a410879a539 76 float motorValue = referenceVelocity / MotorGain;
Arnoud113 5:a3848a66a4df 77
Arnoud113 5:a3848a66a4df 78 // float referenceVelocity = GetReferenceVelocity();
Arnoud113 6:1a410879a539 79
Arnoud113 6:1a410879a539 80 /* if(counts < referencePosition)
Arnoud113 5:a3848a66a4df 81 {
Arnoud113 5:a3848a66a4df 82 //float motorValue = FeedForwardControl(referenceVelocity);
Arnoud113 5:a3848a66a4df 83 //SetMotor1(motorValue);
Arnoud113 5:a3848a66a4df 84 motorValue = referenceVelocity / MotorGain;
Arnoud113 5:a3848a66a4df 85 }
Arnoud113 5:a3848a66a4df 86 else if (counts > referencePosition)
Arnoud113 5:a3848a66a4df 87 {
Arnoud113 5:a3848a66a4df 88 //float motorValue = b * FeedForwardControl(referenceVelocity);
Arnoud113 5:a3848a66a4df 89 //SetMotor1(motorValue);
Arnoud113 5:a3848a66a4df 90
Arnoud113 5:a3848a66a4df 91 motorValue= -1 * (referenceVelocity / MotorGain);
Arnoud113 5:a3848a66a4df 92 }
Arnoud113 5:a3848a66a4df 93 else
Arnoud113 5:a3848a66a4df 94 {
Arnoud113 5:a3848a66a4df 95 motorValue = 0;
Arnoud113 6:1a410879a539 96 }*/
Arnoud113 5:a3848a66a4df 97
Arnoud113 0:1816743ba8ba 98 return motorValue;
Arnoud113 0:1816743ba8ba 99 }
Arnoud113 0:1816743ba8ba 100
Arnoud113 0:1816743ba8ba 101 void SetMotor1(float motorValue)
Arnoud113 0:1816743ba8ba 102 {
Arnoud113 0:1816743ba8ba 103 // Given -1<=motorValue<=1, this sets the PWM and direction
Arnoud113 0:1816743ba8ba 104 // bits for motor 1. Positive value makes motor rotating
Arnoud113 0:1816743ba8ba 105 // clockwise. motorValues outside range are truncated to
Arnoud113 0:1816743ba8ba 106 // within range
Arnoud113 1:659489c32e75 107 if (motorValue >=0) motor1DC= 1;
Arnoud113 0:1816743ba8ba 108 else motor1DC=0;
Arnoud113 0:1816743ba8ba 109 if (fabs(motorValue)>1) motor1PWM = 1;
Arnoud113 0:1816743ba8ba 110 else motor1PWM = fabs(motorValue);
Arnoud113 0:1816743ba8ba 111 }
Arnoud113 0:1816743ba8ba 112
Arnoud113 1:659489c32e75 113 void MeasureAndControl(void)
Arnoud113 1:659489c32e75 114 {
Arnoud113 1:659489c32e75 115 float referenceVelocity = GetReferenceVelocity();
Arnoud113 4:983b50758735 116 int referencePosition = GetReferencePosition();
Arnoud113 5:a3848a66a4df 117 float motorValue = FeedForwardControl(referenceVelocity);
Arnoud113 5:a3848a66a4df 118 SetMotor1(motorValue);
Arnoud113 5:a3848a66a4df 119
Arnoud113 5:a3848a66a4df 120 /*int counts = Encoder.getPulses();
Arnoud113 4:983b50758735 121
Arnoud113 4:983b50758735 122
Arnoud113 4:983b50758735 123
Arnoud113 4:983b50758735 124 if(counts < referencePosition)
Arnoud113 4:983b50758735 125 {
Arnoud113 4:983b50758735 126 float motorValue = FeedForwardControl(referenceVelocity);
Arnoud113 4:983b50758735 127 SetMotor1(motorValue);
Arnoud113 4:983b50758735 128 }
Arnoud113 4:983b50758735 129 else
Arnoud113 4:983b50758735 130 {
Arnoud113 4:983b50758735 131 int b = -1;
Arnoud113 4:983b50758735 132 float motorValue = b * FeedForwardControl(referenceVelocity);
Arnoud113 4:983b50758735 133 SetMotor1(motorValue);
Arnoud113 5:a3848a66a4df 134 } */
Arnoud113 4:983b50758735 135
Arnoud113 1:659489c32e75 136 }
Arnoud113 1:659489c32e75 137
Arnoud113 3:cc3766838777 138
Arnoud113 3:cc3766838777 139
Arnoud113 0:1816743ba8ba 140 int main()
Arnoud113 0:1816743ba8ba 141 {
Arnoud113 0:1816743ba8ba 142 pc.baud(115200);
Arnoud113 2:abd40da4a5e2 143 QEI Encoder(D12,D13,NC,32);
Arnoud113 3:cc3766838777 144
Arnoud113 3:cc3766838777 145 ledr = 1;
Arnoud113 3:cc3766838777 146 ledb = 1;
Arnoud113 3:cc3766838777 147 ledg = 1;
Arnoud113 0:1816743ba8ba 148
Arnoud113 2:abd40da4a5e2 149 controller.attach(&MeasureAndControl, 0.1);
Arnoud113 3:cc3766838777 150
Arnoud113 1:659489c32e75 151 while(1)
Arnoud113 3:cc3766838777 152 {
Arnoud113 2:abd40da4a5e2 153 int counts = Encoder.getPulses();
Arnoud113 2:abd40da4a5e2 154 float rV = GetReferenceVelocity();
Arnoud113 2:abd40da4a5e2 155 float mV = FeedForwardControl(rV);
Arnoud113 4:983b50758735 156 int rP = GetReferencePosition();
Arnoud113 2:abd40da4a5e2 157
Arnoud113 5:a3848a66a4df 158 pc.printf("\r reference velocity: %f. Reference Position: %i Motor Value is: %f number of counts: %i\n",rV,rP,mV,counts);
Arnoud113 2:abd40da4a5e2 159 }
Arnoud113 2:abd40da4a5e2 160
Arnoud113 0:1816743ba8ba 161 }
Arnoud113 0:1816743ba8ba 162
Arnoud113 0:1816743ba8ba 163