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@27:a4228ea8fb8f, 2017-10-13 (annotated)

Committer:
tvlogman
Date:
Fri Oct 13 11:59:15 2017 +0000
Revision:
27:a4228ea8fb8f
Parent:
26:4f84448b4d46
Child:
28:8cd898ff43a2
Most likely fixed motorValue issue, still testing though.

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 27:a4228ea8fb8f 11 const float k_p = 0.5;
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 27:a4228ea8fb8f 36 InterruptIn button2(D4);
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 27:a4228ea8fb8f 121 if(e_pos >= 0){
tvlogman 27:a4228ea8fb8f 122 // Positive error yields positive motor value = counterclockwise rotation
tvlogman 27:a4228ea8fb8f 123 motorValue = -1*((e_pos*k_p) + k_i*e_int + k_d*e_der + 0.45);
tvlogman 27:a4228ea8fb8f 124 }
tvlogman 27:a4228ea8fb8f 125 else {
tvlogman 27:a4228ea8fb8f 126 // Negative error yields negative motor value = clockwise rotation
tvlogman 27:a4228ea8fb8f 127 motorValue = 1*((e_pos*k_p) + k_i*e_int + k_d*e_der + 0.45);
tvlogman 27:a4228ea8fb8f 128 }
tvlogman 27:a4228ea8fb8f 129
tvlogman 19:f08b5cd2b7ce 130 return motorValue;
tvlogman 10:e23cbcdde7e3 131 }
tvlogman 14:664870b5d153 132
tvlogman 19:f08b5cd2b7ce 133 // setMotor1 sets the direction and magnitude pins of motor1 depending on the given motorValue. Negative motorValue means clockwise rotation
tvlogman 19:f08b5cd2b7ce 134 void setMotor1(float motorValue){
tvlogman 19:f08b5cd2b7ce 135 switch(currentState){
tvlogman 19:f08b5cd2b7ce 136 case KILLED:
tvlogman 19:f08b5cd2b7ce 137 motor1_pwm.write(0.0);
tvlogman 22:2e473e9798c0 138 ledR = 0;
tvlogman 22:2e473e9798c0 139 ledG = 1;
tvlogman 22:2e473e9798c0 140 ledB = 1;
tvlogman 19:f08b5cd2b7ce 141 break;
tvlogman 19:f08b5cd2b7ce 142 case ACTIVE:
tvlogman 19:f08b5cd2b7ce 143 // Set motor direction
tvlogman 19:f08b5cd2b7ce 144 if (motorValue >=0){
tvlogman 19:f08b5cd2b7ce 145 motor1_direction = 0;
tvlogman 19:f08b5cd2b7ce 146 }
tvlogman 19:f08b5cd2b7ce 147 else {
tvlogman 19:f08b5cd2b7ce 148 motor1_direction = 1;
tvlogman 19:f08b5cd2b7ce 149 }
tvlogman 19:f08b5cd2b7ce 150 // Set motor speed
tvlogman 19:f08b5cd2b7ce 151 if (fabs(motorValue)>1){
tvlogman 19:f08b5cd2b7ce 152 motor1_pwm = 1;
tvlogman 19:f08b5cd2b7ce 153 }
tvlogman 19:f08b5cd2b7ce 154 else {
tvlogman 19:f08b5cd2b7ce 155 motor1_pwm.write(fabs(motorValue));
tvlogman 22:2e473e9798c0 156 }
tvlogman 22:2e473e9798c0 157 ledR = 1;
tvlogman 22:2e473e9798c0 158 ledG = 1;
tvlogman 22:2e473e9798c0 159 ledB = 0;
tvlogman 19:f08b5cd2b7ce 160 break;
tvlogman 19:f08b5cd2b7ce 161 }
tvlogman 19:f08b5cd2b7ce 162 }
tvlogman 19:f08b5cd2b7ce 163
tvlogman 19:f08b5cd2b7ce 164 void measureAndControl(){
tvlogman 26:4f84448b4d46 165 getError(e_pos, e_int, e_der);
tvlogman 26:4f84448b4d46 166 float motorValue = motorController(e_pos, e_int, e_der);
tvlogman 27:a4228ea8fb8f 167 float r = getReferencePosition();
tvlogman 27:a4228ea8fb8f 168 sendDataToPc(r, e_pos, e_int, e_der, motorValue);
tvlogman 19:f08b5cd2b7ce 169 setMotor1(motorValue);
tvlogman 19:f08b5cd2b7ce 170 }
tvlogman 19:f08b5cd2b7ce 171
tvlogman 14:664870b5d153 172 void killSwitch(){
tvlogman 15:b76b8cff4d8f 173 pc.printf("Motors turned off");
tvlogman 15:b76b8cff4d8f 174 currentState = KILLED;
tvlogman 14:664870b5d153 175 }
tvlogman 14:664870b5d153 176
tvlogman 15:b76b8cff4d8f 177 void turnMotorsOn(){
tvlogman 15:b76b8cff4d8f 178 pc.printf("Motors turned on");
tvlogman 15:b76b8cff4d8f 179 currentState = ACTIVE;
tvlogman 15:b76b8cff4d8f 180 }
tvlogman 15:b76b8cff4d8f 181
tvlogman 27:a4228ea8fb8f 182 void rSwitchDirection(){
tvlogman 27:a4228ea8fb8f 183 r_direction = !r_direction;
tvlogman 27:a4228ea8fb8f 184 pc.printf("Switched reference direction! \r\n");
tvlogman 14:664870b5d153 185 }
vsluiter 0:c8f15874531b 186
tvlogman 21:d266d1e503ce 187
vsluiter 0:c8f15874531b 188 int main()
tvlogman 10:e23cbcdde7e3 189 {
tvlogman 19:f08b5cd2b7ce 190 pc.printf("Main function");
tvlogman 20:4ce3fb543a45 191 motor1_direction = 0; // False = clockwise rotation
tvlogman 13:83e3672b24ee 192 motor1_pwm.period(pwmPeriod);//T=1/f
tvlogman 14:664870b5d153 193 sw2.fall(&killSwitch);
tvlogman 15:b76b8cff4d8f 194 sw3.fall(&turnMotorsOn);
tvlogman 27:a4228ea8fb8f 195 button2.rise(&rSwitchDirection);
vsluiter 0:c8f15874531b 196 pc.baud(115200);
tvlogman 22:2e473e9798c0 197 controllerTicker.attach(measureAndControl, Ts);
tvlogman 15:b76b8cff4d8f 198
tvlogman 19:f08b5cd2b7ce 199 pc.printf("Encoder ticker attached and baudrate set");
vsluiter 0:c8f15874531b 200 }
tvlogman 7:1bffab95fc5f 201