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@7:5b14dbb9e6d1, 2016-10-17 (annotated)

Committer:
s1558382
Date:
Mon Oct 17 14:32:26 2016 +0000
Revision:
7:5b14dbb9e6d1
Parent:
6:7a2c82af02e9
Child:
8:e16b1bd2f344
HIDScope werkt met radians!!

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