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@28:8cd898ff43a2, 2017-10-13 (annotated)

Committer:
tvlogman
Date:
Fri Oct 13 16:24:50 2017 +0000
Revision:
28:8cd898ff43a2
Parent:
27:a4228ea8fb8f
Child:
29:9aa4d63a9bd1
Fixed the direction problem! Changed pin of button2 as it was the same as the motor PWM out.

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 27:a4228ea8fb8f 7 // Defining relevant constant parameters
tvlogman 27:a4228ea8fb8f 8 const float gearRatio = 131;
tvlogman 27:a4228ea8fb8f 9
tvlogman 27:a4228ea8fb8f 10 // Controller parameters
tvlogman 28:8cd898ff43a2 11 const float k_p = 1;
tvlogman 27:a4228ea8fb8f 12 const float k_i = 0; // Still needs a reasonable value
tvlogman 27:a4228ea8fb8f 13 const float k_d = 0; // Again, still need to pick a reasonable value
tvlogman 27:a4228ea8fb8f 14
tvlogman 15:b76b8cff4d8f 15 enum robotStates {KILLED, ACTIVE};
tvlogman 15:b76b8cff4d8f 16 robotStates currentState = KILLED;
tvlogman 8:0067469c3389 17
tvlogman 12:0462757e1ed2 18 QEI Encoder(D12,D13,NC,64, QEI::X4_ENCODING);
tvlogman 10:e23cbcdde7e3 19 MODSERIAL pc(USBTX, USBRX);
tvlogman 27:a4228ea8fb8f 20 HIDScope scope(5);
tvlogman 8:0067469c3389 21
tvlogman 14:664870b5d153 22 // Defining outputs
tvlogman 22:2e473e9798c0 23
tvlogman 22:2e473e9798c0 24 // Leds can be used to indicate status
tvlogman 22:2e473e9798c0 25 DigitalOut ledG(LED_GREEN);
tvlogman 22:2e473e9798c0 26 DigitalOut ledR(LED_RED);
tvlogman 22:2e473e9798c0 27 DigitalOut ledB(LED_BLUE);
tvlogman 22:2e473e9798c0 28
tvlogman 13:83e3672b24ee 29 DigitalOut motor1_direction(D4);
tvlogman 13:83e3672b24ee 30 PwmOut motor1_pwm(D5);
tvlogman 14:664870b5d153 31
tvlogman 14:664870b5d153 32 // Defining inputs
tvlogman 14:664870b5d153 33 InterruptIn sw2(SW2);
tvlogman 15:b76b8cff4d8f 34 InterruptIn sw3(SW3);
tvlogman 16:27430afe663e 35 InterruptIn button1(D2);
tvlogman 28:8cd898ff43a2 36 InterruptIn button2(D3);
tvlogman 21:d266d1e503ce 37 AnalogIn pot1(A0);
tvlogman 21:d266d1e503ce 38 AnalogIn pot2(A1);
tvlogman 15:b76b8cff4d8f 39
tvlogman 16:27430afe663e 40 // PWM settings
tvlogman 16:27430afe663e 41 float pwmPeriod = 1.0/5000.0;
tvlogman 16:27430afe663e 42 int frequency_pwm = 10000; //10kHz PWM
tvlogman 14:664870b5d153 43
tvlogman 27:a4228ea8fb8f 44
tvlogman 27:a4228ea8fb8f 45 // Setting up HIDscope
tvlogman 16:27430afe663e 46 volatile float x;
tvlogman 27:a4228ea8fb8f 47 volatile float y;
tvlogman 27:a4228ea8fb8f 48 volatile float z;
tvlogman 27:a4228ea8fb8f 49 volatile float q;
tvlogman 27:a4228ea8fb8f 50 volatile float k;
tvlogman 27:a4228ea8fb8f 51
tvlogman 27:a4228ea8fb8f 52 void sendDataToPc(float data1, float data2, float data3, float data4, float data5){
tvlogman 27:a4228ea8fb8f 53 // Capture data
tvlogman 27:a4228ea8fb8f 54 x = data1;
tvlogman 27:a4228ea8fb8f 55 y = data2;
tvlogman 27:a4228ea8fb8f 56 z = data3;
tvlogman 27:a4228ea8fb8f 57 q = data4;
tvlogman 27:a4228ea8fb8f 58 k = data5;
tvlogman 27:a4228ea8fb8f 59 scope.set(0, x);
tvlogman 27:a4228ea8fb8f 60 scope.set(1, y);
tvlogman 27:a4228ea8fb8f 61 scope.set(2, z);
tvlogman 27:a4228ea8fb8f 62 scope.set(3, q);
tvlogman 27:a4228ea8fb8f 63 scope.set(4, z);
tvlogman 27:a4228ea8fb8f 64 scope.send(); // send what's in scope memory to PC
tvlogman 27:a4228ea8fb8f 65 }
tvlogman 14:664870b5d153 66
tvlogman 17:616ce7bc1f96 67 // Initializing encoder
tvlogman 10:e23cbcdde7e3 68 Ticker encoderTicker;
tvlogman 19:f08b5cd2b7ce 69 Ticker controllerTicker;
tvlogman 10:e23cbcdde7e3 70 volatile int counts = 0;
tvlogman 12:0462757e1ed2 71 volatile float revs = 0.00;
tvlogman 7:1bffab95fc5f 72
tvlogman 19:f08b5cd2b7ce 73 // MOTOR CONTROL PART
tvlogman 27:a4228ea8fb8f 74 bool r_direction = false;
tvlogman 27:a4228ea8fb8f 75 int posRevRange = 2; // describes the ends of the position range in complete motor output shaft revolutions in both directions
tvlogman 21:d266d1e503ce 76 const float maxAngle = 2*3.14*posRevRange; // max angle in radians
tvlogman 22:2e473e9798c0 77 const float Ts = 0.1;
tvlogman 20:4ce3fb543a45 78
tvlogman 27:a4228ea8fb8f 79 // Function getReferencePosition returns reference angle based on potmeter 1
tvlogman 21:d266d1e503ce 80 float getReferencePosition(){
tvlogman 27:a4228ea8fb8f 81 float r;
tvlogman 27:a4228ea8fb8f 82 if(r_direction == false){
tvlogman 19:f08b5cd2b7ce 83 // Clockwise rotation yields positive reference
tvlogman 21:d266d1e503ce 84 r = maxAngle*pot1.read();
tvlogman 19:f08b5cd2b7ce 85 }
tvlogman 27:a4228ea8fb8f 86 if(r_direction == true){
tvlogman 19:f08b5cd2b7ce 87 // Counterclockwise rotation yields negative reference
tvlogman 21:d266d1e503ce 88 r = -1*maxAngle*pot1.read();
tvlogman 19:f08b5cd2b7ce 89 }
tvlogman 19:f08b5cd2b7ce 90 return r;
tvlogman 19:f08b5cd2b7ce 91 }
tvlogman 19:f08b5cd2b7ce 92
tvlogman 27:a4228ea8fb8f 93 // Initializing position and integral errors to zero
tvlogman 25:df780572cfc8 94 float e_pos = 0;
tvlogman 25:df780572cfc8 95 float e_int = 0;
tvlogman 25:df780572cfc8 96 float e_der = 0;
tvlogman 24:672abc3f02b7 97 float e_prev = 0;
tvlogman 22:2e473e9798c0 98
tvlogman 21:d266d1e503ce 99 // readEncoder reads counts and revs and logs results to serial window
tvlogman 25:df780572cfc8 100 void getError(float &e_pos, float &e_int, float &e_der){
tvlogman 27:a4228ea8fb8f 101 // Getting encoder counts and calculating motor position
tvlogman 21:d266d1e503ce 102 counts = Encoder.getPulses();
tvlogman 27:a4228ea8fb8f 103 double motor1Position = 2*3.14*(counts/(gearRatio*64.0f));
tvlogman 27:a4228ea8fb8f 104
tvlogman 27:a4228ea8fb8f 105 // Computing position error
tvlogman 25:df780572cfc8 106 e_pos = getReferencePosition() - motor1Position;
tvlogman 27:a4228ea8fb8f 107
tvlogman 27:a4228ea8fb8f 108 // Limiting the integral error to prevent integrator saturation
tvlogman 27:a4228ea8fb8f 109 if(fabs(e_int) <= 5){
tvlogman 27:a4228ea8fb8f 110 e_int = e_int + Ts*e_pos;
tvlogman 27:a4228ea8fb8f 111 }
tvlogman 27:a4228ea8fb8f 112
tvlogman 27:a4228ea8fb8f 113 // Derivative error
tvlogman 26:4f84448b4d46 114 e_der = (e_pos - e_prev)/Ts;
tvlogman 27:a4228ea8fb8f 115 e_prev = e_pos; // Store current position error as we'll need it to compute the next derivative error
tvlogman 21:d266d1e503ce 116 }
tvlogman 21:d266d1e503ce 117
tvlogman 21:d266d1e503ce 118 // motorController sets a motorValue based on the position error. Sign indicates direction and magnitude indicates speed. Right now this is a simple feedforward
tvlogman 27:a4228ea8fb8f 119 float motorController(float e_pos, float e_int, float e_der){
tvlogman 27:a4228ea8fb8f 120 float motorValue;
tvlogman 28:8cd898ff43a2 121 motorValue = k_p*e_pos + k_d*e_der + k_i*e_int;
tvlogman 19:f08b5cd2b7ce 122 return motorValue;
tvlogman 10:e23cbcdde7e3 123 }
tvlogman 14:664870b5d153 124
tvlogman 19:f08b5cd2b7ce 125 // setMotor1 sets the direction and magnitude pins of motor1 depending on the given motorValue. Negative motorValue means clockwise rotation
tvlogman 19:f08b5cd2b7ce 126 void setMotor1(float motorValue){
tvlogman 19:f08b5cd2b7ce 127 switch(currentState){
tvlogman 19:f08b5cd2b7ce 128 case KILLED:
tvlogman 19:f08b5cd2b7ce 129 motor1_pwm.write(0.0);
tvlogman 28:8cd898ff43a2 130 // Set motor direction
tvlogman 28:8cd898ff43a2 131 if (motorValue >=0){
tvlogman 28:8cd898ff43a2 132 pc.printf("Fa! \r\n");
tvlogman 28:8cd898ff43a2 133 motor1_direction = 0; //This doesn't seem to set motor 1 direction to 0
tvlogman 28:8cd898ff43a2 134 pc.printf("M1D = %d \r\n", motor1_direction.read());
tvlogman 28:8cd898ff43a2 135 pc.printf("Foo! \r\n");
tvlogman 28:8cd898ff43a2 136 } else if(motorValue < 0){
tvlogman 28:8cd898ff43a2 137 motor1_direction = 1;
tvlogman 28:8cd898ff43a2 138 pc.printf("Bah!");
tvlogman 28:8cd898ff43a2 139 }
tvlogman 22:2e473e9798c0 140 ledR = 0;
tvlogman 22:2e473e9798c0 141 ledG = 1;
tvlogman 22:2e473e9798c0 142 ledB = 1;
tvlogman 19:f08b5cd2b7ce 143 break;
tvlogman 19:f08b5cd2b7ce 144 case ACTIVE:
tvlogman 28:8cd898ff43a2 145 pc.printf("Got into ACTIVE \r\n");
tvlogman 19:f08b5cd2b7ce 146 // Set motor direction
tvlogman 19:f08b5cd2b7ce 147 if (motorValue >=0){
tvlogman 28:8cd898ff43a2 148 pc.printf("Fa!");
tvlogman 28:8cd898ff43a2 149 motor1_direction.write(0);
tvlogman 28:8cd898ff43a2 150 pc.printf("Foo!");
tvlogman 28:8cd898ff43a2 151 } else if(motorValue < 0){
tvlogman 28:8cd898ff43a2 152 motor1_direction.write(1);
tvlogman 28:8cd898ff43a2 153 pc.printf("Bah!");
tvlogman 19:f08b5cd2b7ce 154 }
tvlogman 28:8cd898ff43a2 155
tvlogman 19:f08b5cd2b7ce 156 // Set motor speed
tvlogman 19:f08b5cd2b7ce 157 if (fabs(motorValue)>1){
tvlogman 19:f08b5cd2b7ce 158 motor1_pwm = 1;
tvlogman 19:f08b5cd2b7ce 159 }
tvlogman 19:f08b5cd2b7ce 160 else {
tvlogman 19:f08b5cd2b7ce 161 motor1_pwm.write(fabs(motorValue));
tvlogman 22:2e473e9798c0 162 }
tvlogman 22:2e473e9798c0 163 ledR = 1;
tvlogman 22:2e473e9798c0 164 ledG = 1;
tvlogman 22:2e473e9798c0 165 ledB = 0;
tvlogman 19:f08b5cd2b7ce 166 break;
tvlogman 19:f08b5cd2b7ce 167 }
tvlogman 19:f08b5cd2b7ce 168 }
tvlogman 19:f08b5cd2b7ce 169
tvlogman 19:f08b5cd2b7ce 170 void measureAndControl(){
tvlogman 26:4f84448b4d46 171 getError(e_pos, e_int, e_der);
tvlogman 26:4f84448b4d46 172 float motorValue = motorController(e_pos, e_int, e_der);
tvlogman 27:a4228ea8fb8f 173 float r = getReferencePosition();
tvlogman 27:a4228ea8fb8f 174 sendDataToPc(r, e_pos, e_int, e_der, motorValue);
tvlogman 28:8cd898ff43a2 175 pc.printf("Motorvalue is %.2f \r\n", motorValue);
tvlogman 28:8cd898ff43a2 176 pc.printf("Error is %.2f \r\n", e_pos);
tvlogman 28:8cd898ff43a2 177 pc.printf("Reference is %.2f \r\n", r);
tvlogman 28:8cd898ff43a2 178 pc.printf("motor1 direction is %d \r\n", motor1_direction.read());
tvlogman 19:f08b5cd2b7ce 179 setMotor1(motorValue);
tvlogman 19:f08b5cd2b7ce 180 }
tvlogman 19:f08b5cd2b7ce 181
tvlogman 14:664870b5d153 182 void killSwitch(){
tvlogman 15:b76b8cff4d8f 183 pc.printf("Motors turned off");
tvlogman 15:b76b8cff4d8f 184 currentState = KILLED;
tvlogman 14:664870b5d153 185 }
tvlogman 14:664870b5d153 186
tvlogman 15:b76b8cff4d8f 187 void turnMotorsOn(){
tvlogman 15:b76b8cff4d8f 188 pc.printf("Motors turned on");
tvlogman 15:b76b8cff4d8f 189 currentState = ACTIVE;
tvlogman 15:b76b8cff4d8f 190 }
tvlogman 15:b76b8cff4d8f 191
tvlogman 27:a4228ea8fb8f 192 void rSwitchDirection(){
tvlogman 27:a4228ea8fb8f 193 r_direction = !r_direction;
tvlogman 27:a4228ea8fb8f 194 pc.printf("Switched reference direction! \r\n");
tvlogman 14:664870b5d153 195 }
vsluiter 0:c8f15874531b 196
tvlogman 21:d266d1e503ce 197
vsluiter 0:c8f15874531b 198 int main()
tvlogman 10:e23cbcdde7e3 199 {
tvlogman 19:f08b5cd2b7ce 200 pc.printf("Main function");
tvlogman 20:4ce3fb543a45 201 motor1_direction = 0; // False = clockwise rotation
tvlogman 13:83e3672b24ee 202 motor1_pwm.period(pwmPeriod);//T=1/f
tvlogman 14:664870b5d153 203 sw2.fall(&killSwitch);
tvlogman 15:b76b8cff4d8f 204 sw3.fall(&turnMotorsOn);
tvlogman 27:a4228ea8fb8f 205 button2.rise(&rSwitchDirection);
vsluiter 0:c8f15874531b 206 pc.baud(115200);
tvlogman 22:2e473e9798c0 207 controllerTicker.attach(measureAndControl, Ts);
tvlogman 15:b76b8cff4d8f 208
tvlogman 19:f08b5cd2b7ce 209 pc.printf("Encoder ticker attached and baudrate set");
vsluiter 0:c8f15874531b 210 }
tvlogman 7:1bffab95fc5f 211