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
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?
User | Revision | Line number | New 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 |