Margreet Morsink / Mbed 2 deprecated ControlClaire_motor12

EMG control + motor control of motor 1 + 2

Dependencies:   HIDScope MODSERIAL QEI biquadFilter mbed

Fork of ControlClaire by Margreet Morsink

main.cpp@5:97977cb8daa3, 2016-10-17 (annotated)

Committer:
s1558382
Date:
Mon Oct 17 12:52:55 2016 +0000
Revision:
5:97977cb8daa3
Parent:
4:f1e5bfca905a
Child:
6:7a2c82af02e9
addition of "getposition" not sure if working;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1558382 0:62fe4a2a8101 1 #include "mbed.h"
s1558382 1:43fad4d1dee0 2 #include "MODSERIAL.h"
s1558382 0:62fe4a2a8101 3 #include "QEI.h"
s1558382 0:62fe4a2a8101 4 #include "math.h"
s1558382 0:62fe4a2a8101 5
s1558382 1:43fad4d1dee0 6 //Defining ports
s1558382 2:29d7391d7bc5 7 DigitalOut motor1DirectionPin (D4);
s1558382 2:29d7391d7bc5 8 PwmOut motor1MagnitudePin(D5);
s1558382 1:43fad4d1dee0 9 DigitalIn button(D2);
s1558382 1:43fad4d1dee0 10 AnalogIn potmeter(A0);
s1558382 0:62fe4a2a8101 11 Serial pc(USBTX,USBRX);
s1558382 5:97977cb8daa3 12 QEI encoder(D12,D13,NC,32);
s1558382 0:62fe4a2a8101 13
s1558382 2:29d7391d7bc5 14 // Setting tickers and printers
s1558382 4:f1e5bfca905a 15 Ticker tick;
s1558382 2:29d7391d7bc5 16 Ticker callMotor;
s1558382 2:29d7391d7bc5 17
s1558382 5:97977cb8daa3 18 const float pi = 3.14159265359;
s1558382 1:43fad4d1dee0 19 //Get reference velocity
s1558382 1:43fad4d1dee0 20 float GetReferenceVelocity()
s1558382 0:62fe4a2a8101 21 {
s1558382 1:43fad4d1dee0 22 // Returns reference velocity in rad/s.
s1558382 1:43fad4d1dee0 23 // Positive value means clockwise rotation.
s1558382 1:43fad4d1dee0 24 const float maxVelocity = 8.4; //Als de potmeter van 0 tot 1 gaat (als die maar tot 0.25 gaat, dan max velocity 4x zo groot als motorgain maken
s1558382 1:43fad4d1dee0 25 volatile float referenceVelocity; // in rad/s
s1558382 1:43fad4d1dee0 26 if (button) //nog even kijken voor wanneer die + en - is
s1558382 1:43fad4d1dee0 27 {
s1558382 1:43fad4d1dee0 28 // Clockwise rotation
s1558382 1:43fad4d1dee0 29 referenceVelocity = potmeter * maxVelocity;
s1558382 1:43fad4d1dee0 30 }
s1558382 1:43fad4d1dee0 31 else
s1558382 1:43fad4d1dee0 32 {
s1558382 1:43fad4d1dee0 33 // Counterclockwise rotation
s1558382 1:43fad4d1dee0 34 referenceVelocity = -1*potmeter * maxVelocity;
s1558382 1:43fad4d1dee0 35 }
s1558382 1:43fad4d1dee0 36 return referenceVelocity;
s1558382 0:62fe4a2a8101 37 }
s1558382 0:62fe4a2a8101 38
s1558382 2:29d7391d7bc5 39 void SetMotor1(float motorValue)
s1558382 2:29d7391d7bc5 40 {
s1558382 1:43fad4d1dee0 41 //Given -1<=motorValue<=1, this sets the PWM and direction
s1558382 1:43fad4d1dee0 42 // bits for motor 1. Positive value makes motor rotating
s1558382 1:43fad4d1dee0 43 // clockwise. motorValues outside range are truncated to
s1558382 1:43fad4d1dee0 44 // within range
s1558382 1:43fad4d1dee0 45 if (motorValue >=0)
s1558382 0:62fe4a2a8101 46 {
s1558382 2:29d7391d7bc5 47 motor1DirectionPin=1;
s1558382 1:43fad4d1dee0 48 }
s1558382 0:62fe4a2a8101 49 else
s1558382 0:62fe4a2a8101 50 {
s1558382 2:29d7391d7bc5 51 motor1DirectionPin=0;
s1558382 0:62fe4a2a8101 52 }
s1558382 1:43fad4d1dee0 53 if (fabs(motorValue)>1)
s1558382 1:43fad4d1dee0 54 {
s1558382 2:29d7391d7bc5 55 motor1MagnitudePin = 1;
s1558382 1:43fad4d1dee0 56 }
s1558382 1:43fad4d1dee0 57 else
s1558382 1:43fad4d1dee0 58 {
s1558382 2:29d7391d7bc5 59 motor1MagnitudePin = fabs(motorValue);
s1558382 1:43fad4d1dee0 60 }
s1558382 0:62fe4a2a8101 61 }
s1558382 0:62fe4a2a8101 62
s1558382 0:62fe4a2a8101 63 float FeedForwardControl(float referenceVelocity)
s1558382 0:62fe4a2a8101 64 {
s1558382 1:43fad4d1dee0 65 // very simple linear feed-forward control
s1558382 1:43fad4d1dee0 66 const float MotorGain=8.4; // unit: (rad/s) / PWM
s1558382 1:43fad4d1dee0 67 float motorValue = referenceVelocity / MotorGain;
s1558382 1:43fad4d1dee0 68 return motorValue;
s1558382 1:43fad4d1dee0 69 }
s1558382 1:43fad4d1dee0 70
s1558382 2:29d7391d7bc5 71 void MeasureAndControl(void)
s1558382 0:62fe4a2a8101 72 {
s1558382 1:43fad4d1dee0 73 // This function measures the potmeter position, extracts a
s1558382 1:43fad4d1dee0 74 // reference velocity from it, and controls the motor with
s1558382 1:43fad4d1dee0 75 // a simple FeedForward controller. Call this from a Ticker.
s1558382 1:43fad4d1dee0 76 float referenceVelocity = GetReferenceVelocity();
s1558382 1:43fad4d1dee0 77 float motorValue = FeedForwardControl(referenceVelocity);
s1558382 1:43fad4d1dee0 78 SetMotor1(motorValue);
s1558382 0:62fe4a2a8101 79 }
s1558382 5:97977cb8daa3 80
s1558382 5:97977cb8daa3 81 // position and things w/ encoder & QEI
s1558382 5:97977cb8daa3 82 void getPosition() {
s1558382 5:97977cb8daa3 83 const float ts = 1.0/1000.0;
s1558382 5:97977cb8daa3 84 volatile int pulses = encoder.getPulses();
s1558382 5:97977cb8daa3 85 volatile float radians = (pulses / ts) * ((2*pi)/32);
s1558382 5:97977cb8daa3 86 volatile float prevRadians = radians
s1558382 5:97977cb8daa3 87 volatile float velocity = (radians - prevRadians) / ts;
s1558382 5:97977cb8daa3 88 }
s1558382 2:29d7391d7bc5 89
s1558382 4:f1e5bfca905a 90 void print() {
s1558382 4:f1e5bfca905a 91 pc.printf("Motor value = %f \r\n", FeedForwardControl(GetReferenceVelocity()));
s1558382 4:f1e5bfca905a 92 pc.printf("Reference Velocity = %f \r\n", GetReferenceVelocity());
s1558382 5:97977cb8daa3 93 pc.printf("Radians = %f \r\n", getPosition(radians));
s1558382 5:97977cb8daa3 94 pc.printf("Velocity = %f \r\n", getPosition(velocity));
s1558382 4:f1e5bfca905a 95 }
s1558382 1:43fad4d1dee0 96
s1558382 1:43fad4d1dee0 97 int main()
s1558382 0:62fe4a2a8101 98 {
s1558382 2:29d7391d7bc5 99 motor1MagnitudePin.period(1.0/100000.0);
s1558382 2:29d7391d7bc5 100 callMotor.attach(MeasureAndControl,1);
s1558382 1:43fad4d1dee0 101 pc.baud(115200);
s1558382 4:f1e5bfca905a 102 tick.attach(print,1);
s1558382 5:97977cb8daa3 103 while(1);
s1558382 0:62fe4a2a8101 104 }