Hiz Utwente / Mbed 2 deprecated MotorTest1

Testing motor functions

Dependencies:   HIDScope MODSERIAL QEI mbed

main.cpp@0:e6749361c960, 2017-10-12 (annotated)

Committer:
hiz
Date:
Thu Oct 12 14:21:37 2017 +0000
Revision:
0:e6749361c960
First public, simple motor control

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hiz 0:e6749361c960 1 #include "mbed.h"
hiz 0:e6749361c960 2 #include "MODSERIAL.h"
hiz 0:e6749361c960 3 #include <math.h>
hiz 0:e6749361c960 4 #include "HIDScope.h"
hiz 0:e6749361c960 5 #include "QEI.h"
hiz 0:e6749361c960 6
hiz 0:e6749361c960 7 //Debugging tools
hiz 0:e6749361c960 8 MODSERIAL pc(USBTX, USBRX);
hiz 0:e6749361c960 9
hiz 0:e6749361c960 10 //Declaring pins
hiz 0:e6749361c960 11 AnalogIn potMeterIn(A1);
hiz 0:e6749361c960 12 DigitalIn button1(D2);
hiz 0:e6749361c960 13 InterruptIn button2(D1);
hiz 0:e6749361c960 14
hiz 0:e6749361c960 15 const PinName encoder1 = D12;
hiz 0:e6749361c960 16 const PinName encoder2 = D13;
hiz 0:e6749361c960 17
hiz 0:e6749361c960 18 DigitalOut motor1DirectionPin(D4);
hiz 0:e6749361c960 19 PwmOut motor1MagnitudePin(D5);
hiz 0:e6749361c960 20
hiz 0:e6749361c960 21 //Used for reading out encoder
hiz 0:e6749361c960 22 QEI qei(encoder1, encoder2, NC, 32); //32 is pulses per revolution
hiz 0:e6749361c960 23 HIDScope scope(2);
hiz 0:e6749361c960 24
hiz 0:e6749361c960 25
hiz 0:e6749361c960 26 //Declaring variables
hiz 0:e6749361c960 27 float maxVelocity = 8.4f; // in rad/s
hiz 0:e6749361c960 28 float motorGain = 8.4f; // In rad/s
hiz 0:e6749361c960 29 const float sampletime = 1.0f/10.0f;
hiz 0:e6749361c960 30 bool clockwise = true;
hiz 0:e6749361c960 31 const double pi = 3.14159265358979323846;
hiz 0:e6749361c960 32 volatile bool direction = clockwise;
hiz 0:e6749361c960 33 volatile double lastEncoderRead=0; //In radians
hiz 0:e6749361c960 34
hiz 0:e6749361c960 35 Ticker encoderTicker;
hiz 0:e6749361c960 36 Ticker motorControl;
hiz 0:e6749361c960 37
hiz 0:e6749361c960 38 volatile bool setRotation = true;
hiz 0:e6749361c960 39
hiz 0:e6749361c960 40 float GetReferenceVelocity()
hiz 0:e6749361c960 41 {
hiz 0:e6749361c960 42 // Returns reference velocity in rad/s.
hiz 0:e6749361c960 43 // Positive value means clockwise rotation.
hiz 0:e6749361c960 44 //pc.printf("Begin GetreferenceVelocity\r\n");
hiz 0:e6749361c960 45 float referenceVelocity; // in rad/s
hiz 0:e6749361c960 46 if (button1)
hiz 0:e6749361c960 47 {
hiz 0:e6749361c960 48 // Clockwise rotation
hiz 0:e6749361c960 49 referenceVelocity = potMeterIn * maxVelocity;
hiz 0:e6749361c960 50 }
hiz 0:e6749361c960 51
hiz 0:e6749361c960 52 else
hiz 0:e6749361c960 53 {
hiz 0:e6749361c960 54 // Counterclockwise rotation
hiz 0:e6749361c960 55 referenceVelocity = -1*potMeterIn * maxVelocity;
hiz 0:e6749361c960 56 }
hiz 0:e6749361c960 57 //pc.printf("End Getreferencevelocity\r\n");
hiz 0:e6749361c960 58 return referenceVelocity;
hiz 0:e6749361c960 59 }
hiz 0:e6749361c960 60
hiz 0:e6749361c960 61 void SetMotor1(float motorValue)
hiz 0:e6749361c960 62 {
hiz 0:e6749361c960 63 // Given -1<=motorValue<=1, this sets the PWM and direction
hiz 0:e6749361c960 64 // bits for motor 1. Positive value makes motor rotating
hiz 0:e6749361c960 65 // clockwise. motorValues outside range are truncated to
hiz 0:e6749361c960 66 // within range
hiz 0:e6749361c960 67 //pc.printf("Begin Setmotor1\r\n");
hiz 0:e6749361c960 68 if (motorValue >=0) motor1DirectionPin=1;
hiz 0:e6749361c960 69 else motor1DirectionPin=0;
hiz 0:e6749361c960 70 if (fabs(motorValue)>1) motor1MagnitudePin = 1;
hiz 0:e6749361c960 71 else motor1MagnitudePin = fabs(motorValue);
hiz 0:e6749361c960 72 //pc.printf("End Setmotor1\r\n");
hiz 0:e6749361c960 73 }
hiz 0:e6749361c960 74
hiz 0:e6749361c960 75 float FeedForwardControl(float referenceVelocity)
hiz 0:e6749361c960 76 {
hiz 0:e6749361c960 77 //pc.printf("Begin FeedforwardControl\r\n");
hiz 0:e6749361c960 78 // very simple linear feed-forward control
hiz 0:e6749361c960 79 float motorValue = referenceVelocity / motorGain;
hiz 0:e6749361c960 80 //pc.printf("End Feedforwardcontrol\r\n");
hiz 0:e6749361c960 81 return motorValue;
hiz 0:e6749361c960 82
hiz 0:e6749361c960 83
hiz 0:e6749361c960 84 }
hiz 0:e6749361c960 85
hiz 0:e6749361c960 86 void MeasureAndControl(void)
hiz 0:e6749361c960 87 {
hiz 0:e6749361c960 88 //pc.printf("Begin MeasureAndControl\r\n");
hiz 0:e6749361c960 89 // This function measures the potmeter position, extracts a
hiz 0:e6749361c960 90 // reference velocity from it, and controls the motor with
hiz 0:e6749361c960 91 // a simple FeedForward controller. Call this from a Ticker.
hiz 0:e6749361c960 92 float referenceVelocity = GetReferenceVelocity();
hiz 0:e6749361c960 93 float motorValue = FeedForwardControl(referenceVelocity);
hiz 0:e6749361c960 94 SetMotor1(motorValue);
hiz 0:e6749361c960 95 //pc.printf("End MeasureAndControl\r\n");
hiz 0:e6749361c960 96 }
hiz 0:e6749361c960 97
hiz 0:e6749361c960 98 void encoderTick()
hiz 0:e6749361c960 99 {
hiz 0:e6749361c960 100 int pulses = qei.getPulses();
hiz 0:e6749361c960 101 double radians = (pulses/(32.0f*132.25f))*2.0f*pi;
hiz 0:e6749361c960 102 double degree = (pulses/(32.0f*131.25f))*365;
hiz 0:e6749361c960 103 pc.printf("radians: %d\r\n", radians);
hiz 0:e6749361c960 104 double limit = 360;
hiz 0:e6749361c960 105
hiz 0:e6749361c960 106 if(degree > limit);
hiz 0:e6749361c960 107 {
hiz 0:e6749361c960 108 setRotation =false;
hiz 0:e6749361c960 109 }
hiz 0:e6749361c960 110 if (degree <-limit);
hiz 0:e6749361c960 111 {
hiz 0:e6749361c960 112 setRotation = false;
hiz 0:e6749361c960 113 }
hiz 0:e6749361c960 114
hiz 0:e6749361c960 115 scope.set(0, radians);
hiz 0:e6749361c960 116 scope.set(1, degree);
hiz 0:e6749361c960 117 scope.send();
hiz 0:e6749361c960 118 lastEncoderRead = radians;
hiz 0:e6749361c960 119
hiz 0:e6749361c960 120 }
hiz 0:e6749361c960 121
hiz 0:e6749361c960 122 void status(){
hiz 0:e6749361c960 123 pc.printf("pot input: %f\r\n", potMeterIn.read());
hiz 0:e6749361c960 124 pc.printf("\n\n");
hiz 0:e6749361c960 125 qei.reset();
hiz 0:e6749361c960 126 }
hiz 0:e6749361c960 127
hiz 0:e6749361c960 128 int main(){
hiz 0:e6749361c960 129 pc.printf("Hello World!\r\n");
hiz 0:e6749361c960 130
hiz 0:e6749361c960 131 qei.reset();
hiz 0:e6749361c960 132 encoderTicker.attach(&encoderTick,sampletime);
hiz 0:e6749361c960 133 motorControl.attach(&MeasureAndControl, 1.0f/10.0f);
hiz 0:e6749361c960 134 button2.fall(&status);
hiz 0:e6749361c960 135
hiz 0:e6749361c960 136 while (setRotation)
hiz 0:e6749361c960 137 {
hiz 0:e6749361c960 138 MeasureAndControl();
hiz 0:e6749361c960 139 wait(0.1f);
hiz 0:e6749361c960 140 }
hiz 0:e6749361c960 141 return 0;
hiz 0:e6749361c960 142 }