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@0:62fe4a2a8101, 2016-10-10 (annotated)

Committer:
s1558382
Date:
Mon Oct 10 13:06:24 2016 +0000
Revision:
0:62fe4a2a8101
Child:
1:43fad4d1dee0
Working, button isn't;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
s1558382 0:62fe4a2a8101 1 #include "mbed.h"
s1558382 0:62fe4a2a8101 2 #include "QEI.h"
s1558382 0:62fe4a2a8101 3 #include "math.h"
s1558382 0:62fe4a2a8101 4
s1558382 0:62fe4a2a8101 5 InterruptIn button1(D3);
s1558382 0:62fe4a2a8101 6 AnalogIn potMeterIn(A5);
s1558382 0:62fe4a2a8101 7 DigitalOut motor1DirectionPin(D4);
s1558382 0:62fe4a2a8101 8 PwmOut motor1MagnitudePin(D5);
s1558382 0:62fe4a2a8101 9
s1558382 0:62fe4a2a8101 10 Ticker foutprinter1;
s1558382 0:62fe4a2a8101 11 Ticker foutprinter2;
s1558382 0:62fe4a2a8101 12 Ticker callMotor;
s1558382 0:62fe4a2a8101 13 Serial pc(USBTX,USBRX);
s1558382 0:62fe4a2a8101 14
s1558382 0:62fe4a2a8101 15 volatile float motorValue=0.0;
s1558382 0:62fe4a2a8101 16 volatile float referenceVelocity=0.0; // in rad/s
s1558382 0:62fe4a2a8101 17 const float maxVelocity=8.4; // in rad/s of course!
s1558382 0:62fe4a2a8101 18 const float MotorGain=8.4; // unit: (rad/s) / PWM
s1558382 0:62fe4a2a8101 19
s1558382 0:62fe4a2a8101 20 void foutprint1()
s1558382 0:62fe4a2a8101 21 {
s1558382 0:62fe4a2a8101 22 pc.printf("Motor value = %f\n\r", motorValue);
s1558382 0:62fe4a2a8101 23 }
s1558382 0:62fe4a2a8101 24
s1558382 0:62fe4a2a8101 25 void foutprint2()
s1558382 0:62fe4a2a8101 26 {
s1558382 0:62fe4a2a8101 27 pc.printf("Ref Vel = %f\n\r", referenceVelocity);
s1558382 0:62fe4a2a8101 28 }
s1558382 0:62fe4a2a8101 29
s1558382 0:62fe4a2a8101 30 float getReferenceVelocity()
s1558382 0:62fe4a2a8101 31 {
s1558382 0:62fe4a2a8101 32 // Returns reference velocity in rad/s.
s1558382 0:62fe4a2a8101 33 // Positive value means clockwise rotation.
s1558382 0:62fe4a2a8101 34 if (button1)
s1558382 0:62fe4a2a8101 35 {
s1558382 0:62fe4a2a8101 36 // Clockwise rotation
s1558382 0:62fe4a2a8101 37 referenceVelocity = potMeterIn * maxVelocity;
s1558382 0:62fe4a2a8101 38 }
s1558382 0:62fe4a2a8101 39 else
s1558382 0:62fe4a2a8101 40 {
s1558382 0:62fe4a2a8101 41 // Counterclockwise rotation
s1558382 0:62fe4a2a8101 42 referenceVelocity = -1*potMeterIn * maxVelocity;
s1558382 0:62fe4a2a8101 43 }
s1558382 0:62fe4a2a8101 44 return referenceVelocity;
s1558382 0:62fe4a2a8101 45 }
s1558382 0:62fe4a2a8101 46
s1558382 0:62fe4a2a8101 47 float FeedForwardControl(float referenceVelocity)
s1558382 0:62fe4a2a8101 48 {
s1558382 0:62fe4a2a8101 49 // very simple linear feed-forward control
s1558382 0:62fe4a2a8101 50 motorValue = referenceVelocity / MotorGain;
s1558382 0:62fe4a2a8101 51 return motorValue;
s1558382 0:62fe4a2a8101 52 }
s1558382 0:62fe4a2a8101 53
s1558382 0:62fe4a2a8101 54 void SetMotor1(float motorValue)
s1558382 0:62fe4a2a8101 55 {
s1558382 0:62fe4a2a8101 56 // Given -1<=motorValue<=1, this sets the PWM and direction
s1558382 0:62fe4a2a8101 57 // bits for motor 1. Positive value makes motor rotating
s1558382 0:62fe4a2a8101 58 // clockwise. motorValues outside range are truncated to
s1558382 0:62fe4a2a8101 59 // within range
s1558382 0:62fe4a2a8101 60 if (motorValue >=0.0) motor1DirectionPin=1;
s1558382 0:62fe4a2a8101 61 else motor1DirectionPin=0;
s1558382 0:62fe4a2a8101 62 if (fabs(motorValue)>1) motor1MagnitudePin = 1;
s1558382 0:62fe4a2a8101 63 else motor1MagnitudePin = fabs(motorValue);
s1558382 0:62fe4a2a8101 64 }
s1558382 0:62fe4a2a8101 65
s1558382 0:62fe4a2a8101 66 void MeasureAndControl(void)
s1558382 0:62fe4a2a8101 67 {
s1558382 0:62fe4a2a8101 68 // This function measures the potmeter position, extracts a
s1558382 0:62fe4a2a8101 69 // reference velocity from it, and controls the motor with
s1558382 0:62fe4a2a8101 70 // a simple FeedForward controller. Call this from a Ticker.
s1558382 0:62fe4a2a8101 71 referenceVelocity = getReferenceVelocity();
s1558382 0:62fe4a2a8101 72 motorValue = FeedForwardControl(referenceVelocity);
s1558382 0:62fe4a2a8101 73 SetMotor1(motorValue);
s1558382 0:62fe4a2a8101 74 }
s1558382 0:62fe4a2a8101 75
s1558382 0:62fe4a2a8101 76 int main()
s1558382 0:62fe4a2a8101 77 {
s1558382 0:62fe4a2a8101 78 motor1MagnitudePin.period(1.0);
s1558382 0:62fe4a2a8101 79 foutprinter1.attach(foutprint1,1.0f);
s1558382 0:62fe4a2a8101 80 foutprinter2.attach(foutprint2,1.0f);
s1558382 0:62fe4a2a8101 81 callMotor.attach(MeasureAndControl, 0.1f);
s1558382 0:62fe4a2a8101 82 pc.baud(115200);
s1558382 0:62fe4a2a8101 83 while (true) {
s1558382 0:62fe4a2a8101 84 }
s1558382 0:62fe4a2a8101 85 }