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@30:65f0c9ecf810, 2017-10-16 (annotated)

Committer:
tvlogman
Date:
Mon Oct 16 09:42:04 2017 +0000
Revision:
30:65f0c9ecf810
Parent:
29:9aa4d63a9bd1
Child:
31:cc08254ab7b5
Now with the controller contained in a seperate library;

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