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@31:cc08254ab7b5, 2017-10-17 (annotated)

Committer:
tvlogman
Date:
Tue Oct 17 09:52:12 2017 +0000
Revision:
31:cc08254ab7b5
Parent:
30:65f0c9ecf810
Child:
32:1bb406d2b3c3
Now implementing a working motorConfig library to replace setMotor1 function.

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 31:cc08254ab7b5 8 #include "motorConfig.h"
tvlogman 15:b76b8cff4d8f 9
tvlogman 27:a4228ea8fb8f 10 // Defining relevant constant parameters
tvlogman 27:a4228ea8fb8f 11 const float gearRatio = 131;
tvlogman 27:a4228ea8fb8f 12
tvlogman 27:a4228ea8fb8f 13 // Controller parameters
tvlogman 28:8cd898ff43a2 14 const float k_p = 1;
tvlogman 27:a4228ea8fb8f 15 const float k_i = 0; // Still needs a reasonable value
tvlogman 27:a4228ea8fb8f 16 const float k_d = 0; // Again, still need to pick a reasonable value
tvlogman 27:a4228ea8fb8f 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 inputs
tvlogman 14:664870b5d153 23 InterruptIn sw2(SW2);
tvlogman 15:b76b8cff4d8f 24 InterruptIn sw3(SW3);
tvlogman 16:27430afe663e 25 InterruptIn button1(D2);
tvlogman 28:8cd898ff43a2 26 InterruptIn button2(D3);
tvlogman 21:d266d1e503ce 27 AnalogIn pot2(A1);
tvlogman 15:b76b8cff4d8f 28
tvlogman 16:27430afe663e 29 // PWM settings
tvlogman 16:27430afe663e 30 float pwmPeriod = 1.0/5000.0;
tvlogman 16:27430afe663e 31 int frequency_pwm = 10000; //10kHz PWM
tvlogman 14:664870b5d153 32
tvlogman 27:a4228ea8fb8f 33
tvlogman 27:a4228ea8fb8f 34 // Setting up HIDscope
tvlogman 16:27430afe663e 35 volatile float x;
tvlogman 27:a4228ea8fb8f 36 volatile float y;
tvlogman 27:a4228ea8fb8f 37 volatile float z;
tvlogman 27:a4228ea8fb8f 38 volatile float q;
tvlogman 27:a4228ea8fb8f 39 volatile float k;
tvlogman 27:a4228ea8fb8f 40
tvlogman 27:a4228ea8fb8f 41 void sendDataToPc(float data1, float data2, float data3, float data4, float data5){
tvlogman 27:a4228ea8fb8f 42 // Capture data
tvlogman 27:a4228ea8fb8f 43 x = data1;
tvlogman 27:a4228ea8fb8f 44 y = data2;
tvlogman 27:a4228ea8fb8f 45 z = data3;
tvlogman 27:a4228ea8fb8f 46 q = data4;
tvlogman 27:a4228ea8fb8f 47 k = data5;
tvlogman 27:a4228ea8fb8f 48 scope.set(0, x);
tvlogman 27:a4228ea8fb8f 49 scope.set(1, y);
tvlogman 27:a4228ea8fb8f 50 scope.set(2, z);
tvlogman 27:a4228ea8fb8f 51 scope.set(3, q);
tvlogman 27:a4228ea8fb8f 52 scope.set(4, z);
tvlogman 27:a4228ea8fb8f 53 scope.send(); // send what's in scope memory to PC
tvlogman 27:a4228ea8fb8f 54 }
tvlogman 14:664870b5d153 55
tvlogman 17:616ce7bc1f96 56 // Initializing encoder
tvlogman 10:e23cbcdde7e3 57 Ticker encoderTicker;
tvlogman 19:f08b5cd2b7ce 58 Ticker controllerTicker;
tvlogman 10:e23cbcdde7e3 59 volatile int counts = 0;
tvlogman 12:0462757e1ed2 60 volatile float revs = 0.00;
tvlogman 7:1bffab95fc5f 61
tvlogman 19:f08b5cd2b7ce 62 // MOTOR CONTROL PART
tvlogman 27:a4228ea8fb8f 63 bool r_direction = false;
tvlogman 27:a4228ea8fb8f 64 int posRevRange = 2; // describes the ends of the position range in complete motor output shaft revolutions in both directions
tvlogman 21:d266d1e503ce 65 const float maxAngle = 2*3.14*posRevRange; // max angle in radians
tvlogman 22:2e473e9798c0 66 const float Ts = 0.1;
tvlogman 20:4ce3fb543a45 67
tvlogman 27:a4228ea8fb8f 68 // Function getReferencePosition returns reference angle based on potmeter 1
tvlogman 29:9aa4d63a9bd1 69 refGen ref(A0);
tvlogman 29:9aa4d63a9bd1 70
tvlogman 29:9aa4d63a9bd1 71 //ref.getReferencePosition(maxAngle);
tvlogman 19:f08b5cd2b7ce 72
tvlogman 27:a4228ea8fb8f 73 // Initializing position and integral errors to zero
tvlogman 25:df780572cfc8 74 float e_pos = 0;
tvlogman 25:df780572cfc8 75 float e_int = 0;
tvlogman 25:df780572cfc8 76 float e_der = 0;
tvlogman 24:672abc3f02b7 77 float e_prev = 0;
tvlogman 22:2e473e9798c0 78
tvlogman 21:d266d1e503ce 79 // readEncoder reads counts and revs and logs results to serial window
tvlogman 25:df780572cfc8 80 void getError(float &e_pos, float &e_int, float &e_der){
tvlogman 27:a4228ea8fb8f 81 // Getting encoder counts and calculating motor position
tvlogman 21:d266d1e503ce 82 counts = Encoder.getPulses();
tvlogman 27:a4228ea8fb8f 83 double motor1Position = 2*3.14*(counts/(gearRatio*64.0f));
tvlogman 27:a4228ea8fb8f 84
tvlogman 27:a4228ea8fb8f 85 // Computing position error
tvlogman 29:9aa4d63a9bd1 86 e_pos = ref.getReferencePosition(maxAngle, r_direction) - motor1Position;
tvlogman 27:a4228ea8fb8f 87
tvlogman 27:a4228ea8fb8f 88 // Limiting the integral error to prevent integrator saturation
tvlogman 27:a4228ea8fb8f 89 if(fabs(e_int) <= 5){
tvlogman 27:a4228ea8fb8f 90 e_int = e_int + Ts*e_pos;
tvlogman 27:a4228ea8fb8f 91 }
tvlogman 27:a4228ea8fb8f 92
tvlogman 27:a4228ea8fb8f 93 // Derivative error
tvlogman 26:4f84448b4d46 94 e_der = (e_pos - e_prev)/Ts;
tvlogman 27:a4228ea8fb8f 95 e_prev = e_pos; // Store current position error as we'll need it to compute the next derivative error
tvlogman 21:d266d1e503ce 96 }
tvlogman 21:d266d1e503ce 97
tvlogman 31:cc08254ab7b5 98 // Generate a PID controller with the specified values of k_p, k_d and k_i
tvlogman 30:65f0c9ecf810 99 controller motorController1(k_p, k_d, k_i);
tvlogman 14:664870b5d153 100
tvlogman 19:f08b5cd2b7ce 101 // setMotor1 sets the direction and magnitude pins of motor1 depending on the given motorValue. Negative motorValue means clockwise rotation
tvlogman 31:cc08254ab7b5 102 motorConfig motor1(LED_GREEN,LED_RED,LED_BLUE,D5,D4);
tvlogman 19:f08b5cd2b7ce 103
tvlogman 19:f08b5cd2b7ce 104 void measureAndControl(){
tvlogman 26:4f84448b4d46 105 getError(e_pos, e_int, e_der);
tvlogman 30:65f0c9ecf810 106 float motorValue = motorController1.control(e_pos, e_int, e_der);
tvlogman 29:9aa4d63a9bd1 107 float r = ref.getReferencePosition(maxAngle, r_direction);
tvlogman 27:a4228ea8fb8f 108 sendDataToPc(r, e_pos, e_int, e_der, motorValue);
tvlogman 28:8cd898ff43a2 109 pc.printf("Motorvalue is %.2f \r\n", motorValue);
tvlogman 28:8cd898ff43a2 110 pc.printf("Error is %.2f \r\n", e_pos);
tvlogman 28:8cd898ff43a2 111 pc.printf("Reference is %.2f \r\n", r);
tvlogman 31:cc08254ab7b5 112 //pc.printf("motor1 direction is %d \r\n", motor1_direction.read());
tvlogman 31:cc08254ab7b5 113 motor1.setMotor(motorValue);
tvlogman 15:b76b8cff4d8f 114 }
tvlogman 15:b76b8cff4d8f 115
tvlogman 27:a4228ea8fb8f 116 void rSwitchDirection(){
tvlogman 27:a4228ea8fb8f 117 r_direction = !r_direction;
tvlogman 27:a4228ea8fb8f 118 pc.printf("Switched reference direction! \r\n");
tvlogman 14:664870b5d153 119 }
vsluiter 0:c8f15874531b 120
tvlogman 21:d266d1e503ce 121
vsluiter 0:c8f15874531b 122 int main()
tvlogman 10:e23cbcdde7e3 123 {
tvlogman 19:f08b5cd2b7ce 124 pc.printf("Main function");
tvlogman 31:cc08254ab7b5 125 sw2.fall(&motor1,&motorConfig::killSwitch);
tvlogman 31:cc08254ab7b5 126 sw3.fall(&motor1, &motorConfig::turnMotorOn);
tvlogman 27:a4228ea8fb8f 127 button2.rise(&rSwitchDirection);
vsluiter 0:c8f15874531b 128 pc.baud(115200);
tvlogman 22:2e473e9798c0 129 controllerTicker.attach(measureAndControl, Ts);
tvlogman 15:b76b8cff4d8f 130
tvlogman 19:f08b5cd2b7ce 131 pc.printf("Encoder ticker attached and baudrate set");
vsluiter 0:c8f15874531b 132 }
tvlogman 7:1bffab95fc5f 133