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@25:df780572cfc8, 2017-10-06 (annotated)

Committer:
tvlogman
Date:
Fri Oct 06 12:37:54 2017 +0000
Revision:
25:df780572cfc8
Parent:
24:672abc3f02b7
Child:
26:4f84448b4d46
Changed names of the errors - not tested if it works yet.

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 22:2e473e9798c0 14
tvlogman 22:2e473e9798c0 15 // Leds can be used to indicate status
tvlogman 22:2e473e9798c0 16 DigitalOut ledG(LED_GREEN);
tvlogman 22:2e473e9798c0 17 DigitalOut ledR(LED_RED);
tvlogman 22:2e473e9798c0 18 DigitalOut ledB(LED_BLUE);
tvlogman 22:2e473e9798c0 19
tvlogman 13:83e3672b24ee 20 DigitalOut motor1_direction(D4);
tvlogman 13:83e3672b24ee 21 PwmOut motor1_pwm(D5);
tvlogman 14:664870b5d153 22
tvlogman 14:664870b5d153 23 // Defining inputs
tvlogman 14:664870b5d153 24 InterruptIn sw2(SW2);
tvlogman 15:b76b8cff4d8f 25 InterruptIn sw3(SW3);
tvlogman 16:27430afe663e 26 InterruptIn button1(D2);
tvlogman 21:d266d1e503ce 27 AnalogIn pot1(A0);
tvlogman 21:d266d1e503ce 28 AnalogIn pot2(A1);
tvlogman 15:b76b8cff4d8f 29
tvlogman 16:27430afe663e 30 // PWM settings
tvlogman 16:27430afe663e 31 float pwmPeriod = 1.0/5000.0;
tvlogman 16:27430afe663e 32 int frequency_pwm = 10000; //10kHz PWM
tvlogman 14:664870b5d153 33
tvlogman 16:27430afe663e 34 volatile float x;
tvlogman 16:27430afe663e 35 volatile float x_prev =0;
tvlogman 16:27430afe663e 36 volatile float y; // filtered 'output' of ReadAnalogInAndFilter
tvlogman 14:664870b5d153 37
tvlogman 17:616ce7bc1f96 38 // Initializing encoder
tvlogman 10:e23cbcdde7e3 39 Ticker encoderTicker;
tvlogman 19:f08b5cd2b7ce 40 Ticker controllerTicker;
tvlogman 10:e23cbcdde7e3 41 volatile int counts = 0;
tvlogman 12:0462757e1ed2 42 volatile float revs = 0.00;
tvlogman 7:1bffab95fc5f 43
tvlogman 19:f08b5cd2b7ce 44 // MOTOR CONTROL PART
tvlogman 20:4ce3fb543a45 45 bool m1_direction = false;
tvlogman 24:672abc3f02b7 46 int posRevRange = 5; // describes the ends of the position range in complete motor output shaft revolutions in both directions
tvlogman 21:d266d1e503ce 47 const float maxAngle = 2*3.14*posRevRange; // max angle in radians
tvlogman 22:2e473e9798c0 48 const float Ts = 0.1;
tvlogman 20:4ce3fb543a45 49
tvlogman 21:d266d1e503ce 50 // Function getReferencePosition returns reference angle within range
tvlogman 21:d266d1e503ce 51 float getReferencePosition(){
tvlogman 19:f08b5cd2b7ce 52 double r;
tvlogman 20:4ce3fb543a45 53 if(m1_direction == false){
tvlogman 19:f08b5cd2b7ce 54 // Clockwise rotation yields positive reference
tvlogman 21:d266d1e503ce 55 r = maxAngle*pot1.read();
tvlogman 19:f08b5cd2b7ce 56 }
tvlogman 20:4ce3fb543a45 57 if(m1_direction == true){
tvlogman 19:f08b5cd2b7ce 58 // Counterclockwise rotation yields negative reference
tvlogman 21:d266d1e503ce 59 r = -1*maxAngle*pot1.read();
tvlogman 19:f08b5cd2b7ce 60 }
tvlogman 19:f08b5cd2b7ce 61 return r;
tvlogman 19:f08b5cd2b7ce 62 }
tvlogman 19:f08b5cd2b7ce 63
tvlogman 22:2e473e9798c0 64 // Initializing position and integral errors
tvlogman 25:df780572cfc8 65 float e_pos = 0;
tvlogman 25:df780572cfc8 66 float e_int = 0;
tvlogman 25:df780572cfc8 67 float e_der = 0;
tvlogman 24:672abc3f02b7 68 float e_prev = 0;
tvlogman 25:df780572cfc8 69 float e = e_pos + e_int + e_der;
tvlogman 22:2e473e9798c0 70
tvlogman 21:d266d1e503ce 71 // readEncoder reads counts and revs and logs results to serial window
tvlogman 25:df780572cfc8 72 void getError(float &e_pos, float &e_int, float &e_der){
tvlogman 21:d266d1e503ce 73 counts = Encoder.getPulses();
tvlogman 21:d266d1e503ce 74 double motor1Position = 2*3.14*(counts/(131*64.0f));
tvlogman 21:d266d1e503ce 75 pc.printf("%0.2f revolutions \r\n", motor1Position);
tvlogman 25:df780572cfc8 76 e_pos = getReferencePosition() - motor1Position;
tvlogman 25:df780572cfc8 77 e_int = e_int + Ts*e_pos;
tvlogman 25:df780572cfc8 78 e_der = e_der - e_prev;
tvlogman 25:df780572cfc8 79 e = e_pos + e_int;
tvlogman 25:df780572cfc8 80 pc.printf("Error is %0.2f \r \n", e);
tvlogman 21:d266d1e503ce 81 }
tvlogman 21:d266d1e503ce 82
tvlogman 21:d266d1e503ce 83 // motorController sets a motorValue based on the position error. Sign indicates direction and magnitude indicates speed. Right now this is a simple feedforward
tvlogman 25:df780572cfc8 84 float motorController(float e_pos, float e_int){
tvlogman 21:d266d1e503ce 85 float k_p = 5*pot2.read(); // use potmeter 2 to adjust k_p within range 0 to 1
tvlogman 23:917179f72762 86 float k_i = 0.001; // How do I pick a reasonable value for k_i?
tvlogman 25:df780572cfc8 87 double motorValue = (e_pos*k_p)/maxAngle + 0.35 + k_i*e_int; // 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 88 return motorValue;
tvlogman 10:e23cbcdde7e3 89 }
tvlogman 14:664870b5d153 90
tvlogman 19:f08b5cd2b7ce 91 // setMotor1 sets the direction and magnitude pins of motor1 depending on the given motorValue. Negative motorValue means clockwise rotation
tvlogman 19:f08b5cd2b7ce 92 void setMotor1(float motorValue){
tvlogman 19:f08b5cd2b7ce 93 switch(currentState){
tvlogman 19:f08b5cd2b7ce 94 case KILLED:
tvlogman 19:f08b5cd2b7ce 95 motor1_pwm.write(0.0);
tvlogman 22:2e473e9798c0 96 ledR = 0;
tvlogman 22:2e473e9798c0 97 ledG = 1;
tvlogman 22:2e473e9798c0 98 ledB = 1;
tvlogman 19:f08b5cd2b7ce 99 break;
tvlogman 19:f08b5cd2b7ce 100 case ACTIVE:
tvlogman 19:f08b5cd2b7ce 101 // Set motor direction
tvlogman 19:f08b5cd2b7ce 102 if (motorValue >=0){
tvlogman 19:f08b5cd2b7ce 103 motor1_direction = 0;
tvlogman 19:f08b5cd2b7ce 104 }
tvlogman 19:f08b5cd2b7ce 105 else {
tvlogman 19:f08b5cd2b7ce 106 motor1_direction = 1;
tvlogman 19:f08b5cd2b7ce 107 }
tvlogman 19:f08b5cd2b7ce 108 // Set motor speed
tvlogman 19:f08b5cd2b7ce 109 if (fabs(motorValue)>1){
tvlogman 19:f08b5cd2b7ce 110 motor1_pwm = 1;
tvlogman 19:f08b5cd2b7ce 111 }
tvlogman 19:f08b5cd2b7ce 112 else {
tvlogman 19:f08b5cd2b7ce 113 motor1_pwm.write(fabs(motorValue));
tvlogman 22:2e473e9798c0 114 }
tvlogman 22:2e473e9798c0 115 ledR = 1;
tvlogman 22:2e473e9798c0 116 ledG = 1;
tvlogman 22:2e473e9798c0 117 ledB = 0;
tvlogman 19:f08b5cd2b7ce 118 break;
tvlogman 19:f08b5cd2b7ce 119 }
tvlogman 19:f08b5cd2b7ce 120 }
tvlogman 19:f08b5cd2b7ce 121
tvlogman 19:f08b5cd2b7ce 122 void measureAndControl(){
tvlogman 25:df780572cfc8 123 getError(e_pos, e_int);
tvlogman 25:df780572cfc8 124 float motorValue = motorController(e_pos, e_int);
tvlogman 25:df780572cfc8 125 pc.printf("Position error is %.2f \r \n", e_pos);
tvlogman 25:df780572cfc8 126 pc.printf("Total error is %.2f \r \n", e);
tvlogman 25:df780572cfc8 127 pc.printf("Integral error is %2f", e_int);
tvlogman 22:2e473e9798c0 128 //pc.printf("Motorvalue is %.2f \r \n", motorValue);
tvlogman 22:2e473e9798c0 129 //pc.printf("Motor direction is %d \r \n", motor1_direction);
tvlogman 19:f08b5cd2b7ce 130 setMotor1(motorValue);
tvlogman 19:f08b5cd2b7ce 131 }
tvlogman 19:f08b5cd2b7ce 132
tvlogman 14:664870b5d153 133 void killSwitch(){
tvlogman 15:b76b8cff4d8f 134 pc.printf("Motors turned off");
tvlogman 15:b76b8cff4d8f 135 currentState = KILLED;
tvlogman 14:664870b5d153 136 }
tvlogman 14:664870b5d153 137
tvlogman 15:b76b8cff4d8f 138 void turnMotorsOn(){
tvlogman 15:b76b8cff4d8f 139 pc.printf("Motors turned on");
tvlogman 15:b76b8cff4d8f 140 currentState = ACTIVE;
tvlogman 15:b76b8cff4d8f 141 }
tvlogman 15:b76b8cff4d8f 142
tvlogman 15:b76b8cff4d8f 143
tvlogman 14:664870b5d153 144 void M1switchDirection(){
tvlogman 20:4ce3fb543a45 145 m1_direction = !m1_direction;
tvlogman 22:2e473e9798c0 146 pc.printf("Switched direction!");
tvlogman 14:664870b5d153 147 }
vsluiter 0:c8f15874531b 148
tvlogman 21:d266d1e503ce 149
tvlogman 19:f08b5cd2b7ce 150
vsluiter 0:c8f15874531b 151 int main()
tvlogman 10:e23cbcdde7e3 152 {
tvlogman 19:f08b5cd2b7ce 153 pc.printf("Main function");
tvlogman 20:4ce3fb543a45 154 motor1_direction = 0; // False = clockwise rotation
tvlogman 13:83e3672b24ee 155 motor1_pwm.period(pwmPeriod);//T=1/f
tvlogman 14:664870b5d153 156 sw2.fall(&killSwitch);
tvlogman 15:b76b8cff4d8f 157 sw3.fall(&turnMotorsOn);
tvlogman 16:27430afe663e 158 button1.rise(&M1switchDirection);
vsluiter 0:c8f15874531b 159 pc.baud(115200);
tvlogman 22:2e473e9798c0 160 controllerTicker.attach(measureAndControl, Ts);
tvlogman 15:b76b8cff4d8f 161
tvlogman 19:f08b5cd2b7ce 162 pc.printf("Encoder ticker attached and baudrate set");
vsluiter 0:c8f15874531b 163 }
tvlogman 7:1bffab95fc5f 164