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@29:9aa4d63a9bd1, 2017-10-16 (annotated)

Committer:
tvlogman
Date:
Mon Oct 16 08:45:03 2017 +0000
Revision:
29:9aa4d63a9bd1
Parent:
28:8cd898ff43a2
Child:
30:65f0c9ecf810
Working copy of locomotion_pid_action except this one uses a refGen library to get the reference.

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 15:b76b8cff4d8f 7
tvlogman 27:a4228ea8fb8f 8 // Defining relevant constant parameters
tvlogman 27:a4228ea8fb8f 9 const float gearRatio = 131;
tvlogman 27:a4228ea8fb8f 10
tvlogman 27:a4228ea8fb8f 11 // Controller parameters
tvlogman 28:8cd898ff43a2 12 const float k_p = 1;
tvlogman 27:a4228ea8fb8f 13 const float k_i = 0; // Still needs a reasonable value
tvlogman 27:a4228ea8fb8f 14 const float k_d = 0; // Again, still need to pick a reasonable value
tvlogman 27:a4228ea8fb8f 15
tvlogman 15:b76b8cff4d8f 16 enum robotStates {KILLED, ACTIVE};
tvlogman 15:b76b8cff4d8f 17 robotStates currentState = KILLED;
tvlogman 8:0067469c3389 18
tvlogman 12:0462757e1ed2 19 QEI Encoder(D12,D13,NC,64, QEI::X4_ENCODING);
tvlogman 10:e23cbcdde7e3 20 MODSERIAL pc(USBTX, USBRX);
tvlogman 27:a4228ea8fb8f 21 HIDScope scope(5);
tvlogman 8:0067469c3389 22
tvlogman 14:664870b5d153 23 // Defining outputs
tvlogman 22:2e473e9798c0 24
tvlogman 22:2e473e9798c0 25 // Leds can be used to indicate status
tvlogman 22:2e473e9798c0 26 DigitalOut ledG(LED_GREEN);
tvlogman 22:2e473e9798c0 27 DigitalOut ledR(LED_RED);
tvlogman 22:2e473e9798c0 28 DigitalOut ledB(LED_BLUE);
tvlogman 22:2e473e9798c0 29
tvlogman 13:83e3672b24ee 30 DigitalOut motor1_direction(D4);
tvlogman 13:83e3672b24ee 31 PwmOut motor1_pwm(D5);
tvlogman 14:664870b5d153 32
tvlogman 14:664870b5d153 33 // Defining inputs
tvlogman 14:664870b5d153 34 InterruptIn sw2(SW2);
tvlogman 15:b76b8cff4d8f 35 InterruptIn sw3(SW3);
tvlogman 16:27430afe663e 36 InterruptIn button1(D2);
tvlogman 28:8cd898ff43a2 37 InterruptIn button2(D3);
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 29:9aa4d63a9bd1 80 refGen ref(A0);
tvlogman 29:9aa4d63a9bd1 81
tvlogman 29:9aa4d63a9bd1 82 //ref.getReferencePosition(maxAngle);
tvlogman 19:f08b5cd2b7ce 83
tvlogman 27:a4228ea8fb8f 84 // Initializing position and integral errors to zero
tvlogman 25:df780572cfc8 85 float e_pos = 0;
tvlogman 25:df780572cfc8 86 float e_int = 0;
tvlogman 25:df780572cfc8 87 float e_der = 0;
tvlogman 24:672abc3f02b7 88 float e_prev = 0;
tvlogman 22:2e473e9798c0 89
tvlogman 21:d266d1e503ce 90 // readEncoder reads counts and revs and logs results to serial window
tvlogman 25:df780572cfc8 91 void getError(float &e_pos, float &e_int, float &e_der){
tvlogman 27:a4228ea8fb8f 92 // Getting encoder counts and calculating motor position
tvlogman 21:d266d1e503ce 93 counts = Encoder.getPulses();
tvlogman 27:a4228ea8fb8f 94 double motor1Position = 2*3.14*(counts/(gearRatio*64.0f));
tvlogman 27:a4228ea8fb8f 95
tvlogman 27:a4228ea8fb8f 96 // Computing position error
tvlogman 29:9aa4d63a9bd1 97 e_pos = ref.getReferencePosition(maxAngle, r_direction) - motor1Position;
tvlogman 27:a4228ea8fb8f 98
tvlogman 27:a4228ea8fb8f 99 // Limiting the integral error to prevent integrator saturation
tvlogman 27:a4228ea8fb8f 100 if(fabs(e_int) <= 5){
tvlogman 27:a4228ea8fb8f 101 e_int = e_int + Ts*e_pos;
tvlogman 27:a4228ea8fb8f 102 }
tvlogman 27:a4228ea8fb8f 103
tvlogman 27:a4228ea8fb8f 104 // Derivative error
tvlogman 26:4f84448b4d46 105 e_der = (e_pos - e_prev)/Ts;
tvlogman 27:a4228ea8fb8f 106 e_prev = e_pos; // Store current position error as we'll need it to compute the next derivative error
tvlogman 21:d266d1e503ce 107 }
tvlogman 21:d266d1e503ce 108
tvlogman 21:d266d1e503ce 109 // 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 110 float motorController(float e_pos, float e_int, float e_der){
tvlogman 27:a4228ea8fb8f 111 float motorValue;
tvlogman 28:8cd898ff43a2 112 motorValue = k_p*e_pos + k_d*e_der + k_i*e_int;
tvlogman 19:f08b5cd2b7ce 113 return motorValue;
tvlogman 10:e23cbcdde7e3 114 }
tvlogman 14:664870b5d153 115
tvlogman 19:f08b5cd2b7ce 116 // setMotor1 sets the direction and magnitude pins of motor1 depending on the given motorValue. Negative motorValue means clockwise rotation
tvlogman 19:f08b5cd2b7ce 117 void setMotor1(float motorValue){
tvlogman 19:f08b5cd2b7ce 118 switch(currentState){
tvlogman 19:f08b5cd2b7ce 119 case KILLED:
tvlogman 19:f08b5cd2b7ce 120 motor1_pwm.write(0.0);
tvlogman 28:8cd898ff43a2 121 // Set motor direction
tvlogman 28:8cd898ff43a2 122 if (motorValue >=0){
tvlogman 29:9aa4d63a9bd1 123 // corresponds to CW rotation of motor axle
tvlogman 29:9aa4d63a9bd1 124 motor1_direction = 0;
tvlogman 28:8cd898ff43a2 125 } else if(motorValue < 0){
tvlogman 29:9aa4d63a9bd1 126 // corresponds to CCW rotation of motor axle
tvlogman 28:8cd898ff43a2 127 motor1_direction = 1;
tvlogman 28:8cd898ff43a2 128 pc.printf("Bah!");
tvlogman 28:8cd898ff43a2 129 }
tvlogman 22:2e473e9798c0 130 ledR = 0;
tvlogman 22:2e473e9798c0 131 ledG = 1;
tvlogman 22:2e473e9798c0 132 ledB = 1;
tvlogman 19:f08b5cd2b7ce 133 break;
tvlogman 19:f08b5cd2b7ce 134 case ACTIVE:
tvlogman 28:8cd898ff43a2 135 pc.printf("Got into ACTIVE \r\n");
tvlogman 19:f08b5cd2b7ce 136 // Set motor direction
tvlogman 19:f08b5cd2b7ce 137 if (motorValue >=0){
tvlogman 29:9aa4d63a9bd1 138 // corresponds to CW rotation of motor axle
tvlogman 28:8cd898ff43a2 139 motor1_direction.write(0);
tvlogman 28:8cd898ff43a2 140 } else if(motorValue < 0){
tvlogman 29:9aa4d63a9bd1 141 // corresponds to CCW rotation of motor axle
tvlogman 28:8cd898ff43a2 142 motor1_direction.write(1);
tvlogman 19:f08b5cd2b7ce 143 }
tvlogman 28:8cd898ff43a2 144
tvlogman 19:f08b5cd2b7ce 145 // Set motor speed
tvlogman 19:f08b5cd2b7ce 146 if (fabs(motorValue)>1){
tvlogman 19:f08b5cd2b7ce 147 motor1_pwm = 1;
tvlogman 19:f08b5cd2b7ce 148 }
tvlogman 19:f08b5cd2b7ce 149 else {
tvlogman 29:9aa4d63a9bd1 150 motor1_pwm.write(fabs(motorValue) + 0.4);
tvlogman 22:2e473e9798c0 151 }
tvlogman 22:2e473e9798c0 152 ledR = 1;
tvlogman 22:2e473e9798c0 153 ledG = 1;
tvlogman 22:2e473e9798c0 154 ledB = 0;
tvlogman 19:f08b5cd2b7ce 155 break;
tvlogman 19:f08b5cd2b7ce 156 }
tvlogman 19:f08b5cd2b7ce 157 }
tvlogman 19:f08b5cd2b7ce 158
tvlogman 19:f08b5cd2b7ce 159 void measureAndControl(){
tvlogman 26:4f84448b4d46 160 getError(e_pos, e_int, e_der);
tvlogman 26:4f84448b4d46 161 float motorValue = motorController(e_pos, e_int, e_der);
tvlogman 29:9aa4d63a9bd1 162 float r = ref.getReferencePosition(maxAngle, r_direction);
tvlogman 27:a4228ea8fb8f 163 sendDataToPc(r, e_pos, e_int, e_der, motorValue);
tvlogman 28:8cd898ff43a2 164 pc.printf("Motorvalue is %.2f \r\n", motorValue);
tvlogman 28:8cd898ff43a2 165 pc.printf("Error is %.2f \r\n", e_pos);
tvlogman 28:8cd898ff43a2 166 pc.printf("Reference is %.2f \r\n", r);
tvlogman 28:8cd898ff43a2 167 pc.printf("motor1 direction is %d \r\n", motor1_direction.read());
tvlogman 19:f08b5cd2b7ce 168 setMotor1(motorValue);
tvlogman 19:f08b5cd2b7ce 169 }
tvlogman 19:f08b5cd2b7ce 170
tvlogman 14:664870b5d153 171 void killSwitch(){
tvlogman 15:b76b8cff4d8f 172 pc.printf("Motors turned off");
tvlogman 15:b76b8cff4d8f 173 currentState = KILLED;
tvlogman 14:664870b5d153 174 }
tvlogman 14:664870b5d153 175
tvlogman 15:b76b8cff4d8f 176 void turnMotorsOn(){
tvlogman 15:b76b8cff4d8f 177 pc.printf("Motors turned on");
tvlogman 15:b76b8cff4d8f 178 currentState = ACTIVE;
tvlogman 15:b76b8cff4d8f 179 }
tvlogman 15:b76b8cff4d8f 180
tvlogman 27:a4228ea8fb8f 181 void rSwitchDirection(){
tvlogman 27:a4228ea8fb8f 182 r_direction = !r_direction;
tvlogman 27:a4228ea8fb8f 183 pc.printf("Switched reference direction! \r\n");
tvlogman 14:664870b5d153 184 }
vsluiter 0:c8f15874531b 185
tvlogman 21:d266d1e503ce 186
vsluiter 0:c8f15874531b 187 int main()
tvlogman 10:e23cbcdde7e3 188 {
tvlogman 19:f08b5cd2b7ce 189 pc.printf("Main function");
tvlogman 20:4ce3fb543a45 190 motor1_direction = 0; // False = clockwise rotation
tvlogman 13:83e3672b24ee 191 motor1_pwm.period(pwmPeriod);//T=1/f
tvlogman 14:664870b5d153 192 sw2.fall(&killSwitch);
tvlogman 15:b76b8cff4d8f 193 sw3.fall(&turnMotorsOn);
tvlogman 27:a4228ea8fb8f 194 button2.rise(&rSwitchDirection);
vsluiter 0:c8f15874531b 195 pc.baud(115200);
tvlogman 22:2e473e9798c0 196 controllerTicker.attach(measureAndControl, Ts);
tvlogman 15:b76b8cff4d8f 197
tvlogman 19:f08b5cd2b7ce 198 pc.printf("Encoder ticker attached and baudrate set");
vsluiter 0:c8f15874531b 199 }
tvlogman 7:1bffab95fc5f 200