Tristan Vlogman / Mbed 2 deprecated locomotion_pid_action_refactor_EMG

Control up to two motors using filtered EMG signals and a PID controller

Dependencies:   FastPWM HIDScope MODSERIAL QEI Matrix biquadFilter controller errorFetch mbed motorConfig refGen MatrixMath inverseKinematics

Fork of Minor_test_serial by First Last

main.cpp@21:d266d1e503ce, 2017-10-06 (annotated)

Committer:
tvlogman
Date:
Fri Oct 06 08:34:14 2017 +0000
Revision:
21:d266d1e503ce
Parent:
20:4ce3fb543a45
Child:
22:2e473e9798c0
Working P-controller implemented. Note that setting motor magnitude pin to anything under 0.35 doesn't seem to do much - that needs a fix.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
vsluiter 0:c8f15874531b 1 #include "mbed.h"
vsluiter 0:c8f15874531b 2 #include "MODSERIAL.h"
tvlogman 8:0067469c3389 3 #include "HIDScope.h"
tvlogman 9:5f0e796c9489 4 #include "QEI.h"
tvlogman 15:b76b8cff4d8f 5 #include "FastPWM.h"
tvlogman 15:b76b8cff4d8f 6
tvlogman 15:b76b8cff4d8f 7 enum robotStates {KILLED, ACTIVE};
tvlogman 15:b76b8cff4d8f 8 robotStates currentState = KILLED;
tvlogman 8:0067469c3389 9
tvlogman 12:0462757e1ed2 10 QEI Encoder(D12,D13,NC,64, QEI::X4_ENCODING);
tvlogman 10:e23cbcdde7e3 11 MODSERIAL pc(USBTX, USBRX);
tvlogman 8:0067469c3389 12
tvlogman 14:664870b5d153 13 // Defining outputs
tvlogman 13:83e3672b24ee 14 DigitalOut motor1_direction(D4);
tvlogman 13:83e3672b24ee 15 PwmOut motor1_pwm(D5);
tvlogman 14:664870b5d153 16
tvlogman 14:664870b5d153 17 // Defining inputs
tvlogman 14:664870b5d153 18 InterruptIn sw2(SW2);
tvlogman 15:b76b8cff4d8f 19 InterruptIn sw3(SW3);
tvlogman 16:27430afe663e 20 InterruptIn button1(D2);
tvlogman 21:d266d1e503ce 21 AnalogIn pot1(A0);
tvlogman 21:d266d1e503ce 22 AnalogIn pot2(A1);
tvlogman 15:b76b8cff4d8f 23
tvlogman 16:27430afe663e 24 // PWM settings
tvlogman 16:27430afe663e 25 float pwmPeriod = 1.0/5000.0;
tvlogman 16:27430afe663e 26 int frequency_pwm = 10000; //10kHz PWM
tvlogman 14:664870b5d153 27
tvlogman 16:27430afe663e 28 volatile float x;
tvlogman 16:27430afe663e 29 volatile float x_prev =0;
tvlogman 16:27430afe663e 30 volatile float y; // filtered 'output' of ReadAnalogInAndFilter
tvlogman 14:664870b5d153 31
tvlogman 17:616ce7bc1f96 32 // Initializing encoder
tvlogman 10:e23cbcdde7e3 33 Ticker encoderTicker;
tvlogman 19:f08b5cd2b7ce 34 Ticker controllerTicker;
tvlogman 10:e23cbcdde7e3 35 volatile int counts = 0;
tvlogman 12:0462757e1ed2 36 volatile float revs = 0.00;
tvlogman 7:1bffab95fc5f 37
tvlogman 19:f08b5cd2b7ce 38 // MOTOR CONTROL PART
tvlogman 19:f08b5cd2b7ce 39
tvlogman 20:4ce3fb543a45 40 bool m1_direction = false;
tvlogman 21:d266d1e503ce 41 int posRevRange = 5; // describes the ends of the position range in complete motor output shaft revolutions in both directions
tvlogman 21:d266d1e503ce 42 const float maxAngle = 2*3.14*posRevRange; // max angle in radians
tvlogman 20:4ce3fb543a45 43
tvlogman 21:d266d1e503ce 44 // Function getReferencePosition returns reference angle within range
tvlogman 21:d266d1e503ce 45 float getReferencePosition(){
tvlogman 19:f08b5cd2b7ce 46 double r;
tvlogman 20:4ce3fb543a45 47 if(m1_direction == false){
tvlogman 19:f08b5cd2b7ce 48 // Clockwise rotation yields positive reference
tvlogman 21:d266d1e503ce 49 r = maxAngle*pot1.read();
tvlogman 19:f08b5cd2b7ce 50 }
tvlogman 20:4ce3fb543a45 51 if(m1_direction == true){
tvlogman 19:f08b5cd2b7ce 52 // Counterclockwise rotation yields negative reference
tvlogman 21:d266d1e503ce 53 r = -1*maxAngle*pot1.read();
tvlogman 19:f08b5cd2b7ce 54 }
tvlogman 19:f08b5cd2b7ce 55 return r;
tvlogman 19:f08b5cd2b7ce 56 }
tvlogman 19:f08b5cd2b7ce 57
tvlogman 21:d266d1e503ce 58 // readEncoder reads counts and revs and logs results to serial window
tvlogman 21:d266d1e503ce 59 float getError(){
tvlogman 21:d266d1e503ce 60 counts = Encoder.getPulses();
tvlogman 21:d266d1e503ce 61 double motor1Position = 2*3.14*(counts/(131*64.0f));
tvlogman 21:d266d1e503ce 62 pc.printf("%0.2f revolutions \r\n", motor1Position);
tvlogman 21:d266d1e503ce 63 float posError = getReferencePosition() - motor1Position;
tvlogman 21:d266d1e503ce 64 pc.printf("Error is %0.2f \r \n", posError);
tvlogman 21:d266d1e503ce 65 return posError;
tvlogman 21:d266d1e503ce 66 }
tvlogman 21:d266d1e503ce 67
tvlogman 21:d266d1e503ce 68 // motorController sets a motorValue based on the position error. Sign indicates direction and magnitude indicates speed. Right now this is a simple feedforward
tvlogman 21:d266d1e503ce 69 float motorController(float posError){
tvlogman 21:d266d1e503ce 70 float k_p = 5*pot2.read(); // use potmeter 2 to adjust k_p within range 0 to 1
tvlogman 21:d266d1e503ce 71 double motorValue = (posError*k_p)/maxAngle + 0.35; // motorValue is the normalized voltage applied to motor magnitude pin between -1 and 1 added 0.35 as motorvalue of 0.35 does nothing
tvlogman 19:f08b5cd2b7ce 72 return motorValue;
tvlogman 10:e23cbcdde7e3 73 }
tvlogman 14:664870b5d153 74
tvlogman 19:f08b5cd2b7ce 75 // setMotor1 sets the direction and magnitude pins of motor1 depending on the given motorValue. Negative motorValue means clockwise rotation
tvlogman 19:f08b5cd2b7ce 76 void setMotor1(float motorValue){
tvlogman 19:f08b5cd2b7ce 77 switch(currentState){
tvlogman 19:f08b5cd2b7ce 78 case KILLED:
tvlogman 19:f08b5cd2b7ce 79 motor1_pwm.write(0.0);
tvlogman 19:f08b5cd2b7ce 80 break;
tvlogman 19:f08b5cd2b7ce 81 case ACTIVE:
tvlogman 19:f08b5cd2b7ce 82 // Set motor direction
tvlogman 19:f08b5cd2b7ce 83 if (motorValue >=0){
tvlogman 19:f08b5cd2b7ce 84 motor1_direction = 0;
tvlogman 19:f08b5cd2b7ce 85 }
tvlogman 19:f08b5cd2b7ce 86 else {
tvlogman 19:f08b5cd2b7ce 87 motor1_direction = 1;
tvlogman 19:f08b5cd2b7ce 88 }
tvlogman 19:f08b5cd2b7ce 89 // Set motor speed
tvlogman 19:f08b5cd2b7ce 90 if (fabs(motorValue)>1){
tvlogman 19:f08b5cd2b7ce 91 motor1_pwm = 1;
tvlogman 19:f08b5cd2b7ce 92 }
tvlogman 19:f08b5cd2b7ce 93 else {
tvlogman 19:f08b5cd2b7ce 94 motor1_pwm.write(fabs(motorValue));
tvlogman 19:f08b5cd2b7ce 95 }
tvlogman 19:f08b5cd2b7ce 96 break;
tvlogman 19:f08b5cd2b7ce 97 }
tvlogman 19:f08b5cd2b7ce 98 }
tvlogman 19:f08b5cd2b7ce 99
tvlogman 19:f08b5cd2b7ce 100 void measureAndControl(){
tvlogman 21:d266d1e503ce 101 float posError = getError();
tvlogman 21:d266d1e503ce 102 float motorValue = motorController(posError);
tvlogman 19:f08b5cd2b7ce 103 pc.printf("Motorvalue is %.2f \r \n", motorValue);
tvlogman 21:d266d1e503ce 104 pc.printf("Position error is %.2f \r \n", posError);
tvlogman 19:f08b5cd2b7ce 105 pc.printf("Motor direction is %d \r \n", motor1_direction);
tvlogman 19:f08b5cd2b7ce 106 setMotor1(motorValue);
tvlogman 19:f08b5cd2b7ce 107 }
tvlogman 19:f08b5cd2b7ce 108
tvlogman 14:664870b5d153 109 void killSwitch(){
tvlogman 15:b76b8cff4d8f 110 pc.printf("Motors turned off");
tvlogman 15:b76b8cff4d8f 111 currentState = KILLED;
tvlogman 14:664870b5d153 112 }
tvlogman 14:664870b5d153 113
tvlogman 15:b76b8cff4d8f 114 void turnMotorsOn(){
tvlogman 15:b76b8cff4d8f 115 pc.printf("Motors turned on");
tvlogman 15:b76b8cff4d8f 116 currentState = ACTIVE;
tvlogman 15:b76b8cff4d8f 117 }
tvlogman 15:b76b8cff4d8f 118
tvlogman 15:b76b8cff4d8f 119
tvlogman 14:664870b5d153 120 void M1switchDirection(){
tvlogman 20:4ce3fb543a45 121 m1_direction = !m1_direction;
tvlogman 14:664870b5d153 122 }
vsluiter 0:c8f15874531b 123
tvlogman 21:d266d1e503ce 124
tvlogman 19:f08b5cd2b7ce 125
vsluiter 0:c8f15874531b 126 int main()
tvlogman 10:e23cbcdde7e3 127 {
tvlogman 19:f08b5cd2b7ce 128 pc.printf("Main function");
tvlogman 20:4ce3fb543a45 129 motor1_direction = 0; // False = clockwise rotation
tvlogman 13:83e3672b24ee 130 motor1_pwm.period(pwmPeriod);//T=1/f
tvlogman 14:664870b5d153 131 sw2.fall(&killSwitch);
tvlogman 15:b76b8cff4d8f 132 sw3.fall(&turnMotorsOn);
tvlogman 16:27430afe663e 133 button1.rise(&M1switchDirection);
vsluiter 0:c8f15874531b 134 pc.baud(115200);
tvlogman 19:f08b5cd2b7ce 135 controllerTicker.attach(measureAndControl, 0.1);
tvlogman 15:b76b8cff4d8f 136
tvlogman 19:f08b5cd2b7ce 137 pc.printf("Encoder ticker attached and baudrate set");
vsluiter 0:c8f15874531b 138 }
tvlogman 7:1bffab95fc5f 139