Arnoud Meutstege / Mbed 2 deprecated Tut5_motor_controller

first itteration

Dependencies:   MODSERIAL QEI mbed

main.cpp@0:1816743ba8ba, 2017-10-04 (annotated)

Committer:
Arnoud113
Date:
Wed Oct 04 12:46:37 2017 +0000
Revision:
0:1816743ba8ba
Child:
1:659489c32e75
Original versioin which runs the engine to one side and stops the engine when but 1 in pressed. However, the engine keeps on running with the button pressed when the controlling pot meter is turned to zero value.

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 0:1816743ba8ba 10 DigitalOut motor1DC(D6);
Arnoud113 0:1816743ba8ba 11 DigitalOut motor1PWM(D7);
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 0:1816743ba8ba 19
Arnoud113 0:1816743ba8ba 20 float GetReferenceVelocity()
Arnoud113 0:1816743ba8ba 21 {
Arnoud113 0:1816743ba8ba 22 // Returns reference velocity in rad/s.
Arnoud113 0:1816743ba8ba 23 // Positive value means clockwise rotation.
Arnoud113 0:1816743ba8ba 24
Arnoud113 0:1816743ba8ba 25 const float maxVelocity=8.4; // in rad/s of course!
Arnoud113 0:1816743ba8ba 26
Arnoud113 0:1816743ba8ba 27 float referenceVelocity; // in rad/s
Arnoud113 0:1816743ba8ba 28
Arnoud113 0:1816743ba8ba 29 if (button1) {
Arnoud113 0:1816743ba8ba 30 // Clockwise rotation
Arnoud113 0:1816743ba8ba 31 referenceVelocity = potMeterIn * maxVelocity;
Arnoud113 0:1816743ba8ba 32 }
Arnoud113 0:1816743ba8ba 33 else {
Arnoud113 0:1816743ba8ba 34 // Counterclockwise rotation
Arnoud113 0:1816743ba8ba 35 referenceVelocity = -1*potMeterIn * maxVelocity;
Arnoud113 0:1816743ba8ba 36 }
Arnoud113 0:1816743ba8ba 37 return referenceVelocity;
Arnoud113 0:1816743ba8ba 38 }
Arnoud113 0:1816743ba8ba 39
Arnoud113 0:1816743ba8ba 40
Arnoud113 0:1816743ba8ba 41 float FeedForwardControl(float &referenceVelocity)
Arnoud113 0:1816743ba8ba 42 {
Arnoud113 0:1816743ba8ba 43 // very simple linear feed-forward control
Arnoud113 0:1816743ba8ba 44 const float MotorGain=8.4; // unit: (rad/s) / PWM
Arnoud113 0:1816743ba8ba 45 float motorValue = referenceVelocity / MotorGain;
Arnoud113 0:1816743ba8ba 46 return motorValue;
Arnoud113 0:1816743ba8ba 47 }
Arnoud113 0:1816743ba8ba 48
Arnoud113 0:1816743ba8ba 49 void SetMotor1(float motorValue)
Arnoud113 0:1816743ba8ba 50 {
Arnoud113 0:1816743ba8ba 51 // Given -1<=motorValue<=1, this sets the PWM and direction
Arnoud113 0:1816743ba8ba 52 // bits for motor 1. Positive value makes motor rotating
Arnoud113 0:1816743ba8ba 53 // clockwise. motorValues outside range are truncated to
Arnoud113 0:1816743ba8ba 54 // within range
Arnoud113 0:1816743ba8ba 55 if (motorValue >=0) motor1DC=1;
Arnoud113 0:1816743ba8ba 56 else motor1DC=0;
Arnoud113 0:1816743ba8ba 57 if (fabs(motorValue)>1) motor1PWM = 1;
Arnoud113 0:1816743ba8ba 58 else motor1PWM = fabs(motorValue);
Arnoud113 0:1816743ba8ba 59 }
Arnoud113 0:1816743ba8ba 60
Arnoud113 0:1816743ba8ba 61 int main()
Arnoud113 0:1816743ba8ba 62 {
Arnoud113 0:1816743ba8ba 63 pc.baud(115200);
Arnoud113 0:1816743ba8ba 64
Arnoud113 0:1816743ba8ba 65 QEI Encoder(D12,D13,NC,32); // Input for the Encoder
Arnoud113 0:1816743ba8ba 66
Arnoud113 0:1816743ba8ba 67 while (true) {
Arnoud113 0:1816743ba8ba 68
Arnoud113 0:1816743ba8ba 69 float v = GetReferenceVelocity();
Arnoud113 0:1816743ba8ba 70 float controlValue = FeedForwardControl(v);
Arnoud113 0:1816743ba8ba 71 SetMotor1(controlValue);
Arnoud113 0:1816743ba8ba 72
Arnoud113 0:1816743ba8ba 73 int counts;
Arnoud113 0:1816743ba8ba 74 counts = Encoder.getPulses();
Arnoud113 0:1816743ba8ba 75 pc.printf("\r number of counts: %i\n",counts);
Arnoud113 0:1816743ba8ba 76
Arnoud113 0:1816743ba8ba 77 pc.printf("\t\r reference velocity is: %f. Motor Value is: %f \t\r number of counts: %i\n",v,controlValue,counts);
Arnoud113 0:1816743ba8ba 78 }
Arnoud113 0:1816743ba8ba 79 }
Arnoud113 0:1816743ba8ba 80
Arnoud113 0:1816743ba8ba 81