Arnoud Meutstege / Mbed 2 deprecated Tut5_motor_controller

first itteration

Dependencies:   MODSERIAL QEI mbed

main.cpp@5:a3848a66a4df, 2017-10-06 (annotated)

Committer:
Arnoud113
Date:
Fri Oct 06 11:52:13 2017 +0000
Revision:
5:a3848a66a4df
Parent:
4:983b50758735
Child:
6:1a410879a539
not working version of p attempt

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