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@1:43fad4d1dee0, 2016-10-16 (annotated)

Committer:
s1558382
Date:
Sun Oct 16 13:24:33 2016 +0000
Revision:
1:43fad4d1dee0
Parent:
0:62fe4a2a8101
Child:
2:29d7391d7bc5
Button works; printing out values gives values = 0.

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 0:62fe4a2a8101 6
s1558382 1:43fad4d1dee0 7 //Defining ports
s1558382 1:43fad4d1dee0 8 DigitalOut motordirect (D4);
s1558382 1:43fad4d1dee0 9 PwmOut motorpwm (D5);
s1558382 1:43fad4d1dee0 10 DigitalIn button(D2);
s1558382 1:43fad4d1dee0 11 AnalogIn potmeter(A0);
s1558382 0:62fe4a2a8101 12 Serial pc(USBTX,USBRX);
s1558382 0:62fe4a2a8101 13
s1558382 1:43fad4d1dee0 14 //Get reference velocity
s1558382 1:43fad4d1dee0 15 volatile float referenceVelocity; // in rad/s
s1558382 1:43fad4d1dee0 16 float GetReferenceVelocity()
s1558382 0:62fe4a2a8101 17 {
s1558382 1:43fad4d1dee0 18 // Returns reference velocity in rad/s.
s1558382 1:43fad4d1dee0 19 // Positive value means clockwise rotation.
s1558382 1:43fad4d1dee0 20 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 21 volatile float referenceVelocity; // in rad/s
s1558382 1:43fad4d1dee0 22 if (button) //nog even kijken voor wanneer die + en - is
s1558382 1:43fad4d1dee0 23 {
s1558382 1:43fad4d1dee0 24 // Clockwise rotation
s1558382 1:43fad4d1dee0 25 referenceVelocity = potmeter * maxVelocity;
s1558382 1:43fad4d1dee0 26 }
s1558382 1:43fad4d1dee0 27 else
s1558382 1:43fad4d1dee0 28 {
s1558382 1:43fad4d1dee0 29 // Counterclockwise rotation
s1558382 1:43fad4d1dee0 30 referenceVelocity = -1*potmeter * maxVelocity;
s1558382 1:43fad4d1dee0 31 }
s1558382 1:43fad4d1dee0 32 return referenceVelocity;
s1558382 0:62fe4a2a8101 33 }
s1558382 0:62fe4a2a8101 34
s1558382 1:43fad4d1dee0 35 volatile float motorValue;
s1558382 1:43fad4d1dee0 36 void SetMotor1(float motorValue){
s1558382 1:43fad4d1dee0 37 //Given -1<=motorValue<=1, this sets the PWM and direction
s1558382 1:43fad4d1dee0 38 // bits for motor 1. Positive value makes motor rotating
s1558382 1:43fad4d1dee0 39 // clockwise. motorValues outside range are truncated to
s1558382 1:43fad4d1dee0 40 // within range
s1558382 1:43fad4d1dee0 41 if (motorValue >=0)
s1558382 0:62fe4a2a8101 42 {
s1558382 1:43fad4d1dee0 43 motordirect=1;
s1558382 1:43fad4d1dee0 44 }
s1558382 0:62fe4a2a8101 45 else
s1558382 0:62fe4a2a8101 46 {
s1558382 1:43fad4d1dee0 47 motordirect=0;
s1558382 0:62fe4a2a8101 48 }
s1558382 1:43fad4d1dee0 49 if (fabs(motorValue)>1)
s1558382 1:43fad4d1dee0 50 {
s1558382 1:43fad4d1dee0 51 motorpwm = 1;
s1558382 1:43fad4d1dee0 52 }
s1558382 1:43fad4d1dee0 53 else
s1558382 1:43fad4d1dee0 54 {
s1558382 1:43fad4d1dee0 55 motorpwm = fabs(motorValue);
s1558382 1:43fad4d1dee0 56 }
s1558382 0:62fe4a2a8101 57 }
s1558382 0:62fe4a2a8101 58
s1558382 0:62fe4a2a8101 59 float FeedForwardControl(float referenceVelocity)
s1558382 0:62fe4a2a8101 60 {
s1558382 1:43fad4d1dee0 61 // very simple linear feed-forward control
s1558382 1:43fad4d1dee0 62 const float MotorGain=8.4; // unit: (rad/s) / PWM
s1558382 1:43fad4d1dee0 63 float motorValue = referenceVelocity / MotorGain;
s1558382 1:43fad4d1dee0 64 return motorValue;
s1558382 1:43fad4d1dee0 65 }
s1558382 1:43fad4d1dee0 66
s1558382 1:43fad4d1dee0 67 void MeasureAndControl()
s1558382 0:62fe4a2a8101 68 {
s1558382 1:43fad4d1dee0 69 // This function measures the potmeter position, extracts a
s1558382 1:43fad4d1dee0 70 // reference velocity from it, and controls the motor with
s1558382 1:43fad4d1dee0 71 // a simple FeedForward controller. Call this from a Ticker.
s1558382 1:43fad4d1dee0 72 float referenceVelocity = GetReferenceVelocity();
s1558382 1:43fad4d1dee0 73 float motorValue = FeedForwardControl(referenceVelocity);
s1558382 1:43fad4d1dee0 74 SetMotor1(motorValue);
s1558382 0:62fe4a2a8101 75 }
s1558382 0:62fe4a2a8101 76
s1558382 1:43fad4d1dee0 77 // Setting tickers and printers
s1558382 1:43fad4d1dee0 78 Ticker motorTick;
s1558382 1:43fad4d1dee0 79 Ticker velocityTick;
s1558382 1:43fad4d1dee0 80
s1558382 1:43fad4d1dee0 81 void motorPrint(){
s1558382 1:43fad4d1dee0 82 pc.printf("Motor value = %f \r\n",motorValue);
s1558382 1:43fad4d1dee0 83 }
s1558382 1:43fad4d1dee0 84
s1558382 1:43fad4d1dee0 85 void velocityPrint(){
s1558382 1:43fad4d1dee0 86 pc.printf("Reference velocity = %f \r\n",referenceVelocity);
s1558382 1:43fad4d1dee0 87 }
s1558382 1:43fad4d1dee0 88
s1558382 1:43fad4d1dee0 89 int main()
s1558382 0:62fe4a2a8101 90 {
s1558382 1:43fad4d1dee0 91 motorpwm.period(1.0/1000.0);
s1558382 1:43fad4d1dee0 92 Ticker motordraaien;
s1558382 1:43fad4d1dee0 93 motordraaien.attach(MeasureAndControl,1);
s1558382 1:43fad4d1dee0 94 pc.baud(115200);
s1558382 1:43fad4d1dee0 95 motorTick.attach(motorPrint,1);
s1558382 1:43fad4d1dee0 96 velocityTick.attach(velocityPrint,1);
s1558382 1:43fad4d1dee0 97 while(true){}
s1558382 0:62fe4a2a8101 98 }