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@33:6f4858b98fe5, 2017-10-20 (annotated)

Committer:
tvlogman
Date:
Fri Oct 20 16:30:26 2017 +0000
Revision:
33:6f4858b98fe5
Parent:
32:1bb406d2b3c3
Child:
34:1a70aa045c8f
Now works implementing a modular error-getting system

Who changed what in which revision?

UserRevisionLine numberNew contents of line
tvlogman 33:6f4858b98fe5 1 #include <vector>
vsluiter 0:c8f15874531b 2 #include "mbed.h"
vsluiter 0:c8f15874531b 3 #include "MODSERIAL.h"
tvlogman 8:0067469c3389 4 #include "HIDScope.h"
tvlogman 9:5f0e796c9489 5 #include "QEI.h"
tvlogman 15:b76b8cff4d8f 6 #include "FastPWM.h"
tvlogman 29:9aa4d63a9bd1 7 #include "refGen.h"
tvlogman 30:65f0c9ecf810 8 #include "controller.h"
tvlogman 31:cc08254ab7b5 9 #include "motorConfig.h"
tvlogman 32:1bb406d2b3c3 10 #include "errorFetch.h"
tvlogman 15:b76b8cff4d8f 11
tvlogman 33:6f4858b98fe5 12 // ADJUSTABLE PARAMETERS
tvlogman 27:a4228ea8fb8f 13
tvlogman 27:a4228ea8fb8f 14 // Controller parameters
tvlogman 28:8cd898ff43a2 15 const float k_p = 1;
tvlogman 27:a4228ea8fb8f 16 const float k_i = 0; // Still needs a reasonable value
tvlogman 27:a4228ea8fb8f 17 const float k_d = 0; // Again, still need to pick a reasonable value
tvlogman 27:a4228ea8fb8f 18
tvlogman 33:6f4858b98fe5 19 // Defining motor gear ratio - for BOTH motors as this is the same in the current configuration
tvlogman 33:6f4858b98fe5 20 const float gearRatio = 131;
tvlogman 33:6f4858b98fe5 21
tvlogman 33:6f4858b98fe5 22
tvlogman 33:6f4858b98fe5 23
tvlogman 33:6f4858b98fe5 24 // Declaring a controller ticker and volatile variables to store encoder counts and revs
tvlogman 33:6f4858b98fe5 25 Ticker controllerTicker;
tvlogman 33:6f4858b98fe5 26 volatile int m1counts = 0;
tvlogman 33:6f4858b98fe5 27 volatile int m2counts = 0;
tvlogman 33:6f4858b98fe5 28 volatile float m1revs = 0.00;
tvlogman 33:6f4858b98fe5 29 volatile float m2revs = 0.00;
tvlogman 33:6f4858b98fe5 30
tvlogman 33:6f4858b98fe5 31 // PWM settings
tvlogman 33:6f4858b98fe5 32 float pwmPeriod = 1.0/5000.0;
tvlogman 33:6f4858b98fe5 33 int frequency_pwm = 10000; //10kHz PWM
tvlogman 33:6f4858b98fe5 34
tvlogman 33:6f4858b98fe5 35 // Initializing encoder
tvlogman 32:1bb406d2b3c3 36 QEI Encoder1(D12,D13,NC,64, QEI::X4_ENCODING);
tvlogman 32:1bb406d2b3c3 37 QEI Encoder2(D11,D10,NC,64, QEI::X4_ENCODING);
tvlogman 10:e23cbcdde7e3 38 MODSERIAL pc(USBTX, USBRX);
tvlogman 27:a4228ea8fb8f 39 HIDScope scope(5);
tvlogman 8:0067469c3389 40
tvlogman 14:664870b5d153 41 // Defining inputs
tvlogman 14:664870b5d153 42 InterruptIn sw2(SW2);
tvlogman 15:b76b8cff4d8f 43 InterruptIn sw3(SW3);
tvlogman 16:27430afe663e 44 InterruptIn button1(D2);
tvlogman 28:8cd898ff43a2 45 InterruptIn button2(D3);
tvlogman 32:1bb406d2b3c3 46 AnalogIn pot2(A3);
tvlogman 32:1bb406d2b3c3 47 AnalogIn emg0( A0 );
tvlogman 32:1bb406d2b3c3 48 //AnalogIn emg1( A1 );
tvlogman 15:b76b8cff4d8f 49
tvlogman 27:a4228ea8fb8f 50 // Setting up HIDscope
tvlogman 16:27430afe663e 51 volatile float x;
tvlogman 27:a4228ea8fb8f 52 volatile float y;
tvlogman 27:a4228ea8fb8f 53 volatile float z;
tvlogman 27:a4228ea8fb8f 54 volatile float q;
tvlogman 27:a4228ea8fb8f 55 volatile float k;
tvlogman 27:a4228ea8fb8f 56
tvlogman 27:a4228ea8fb8f 57 void sendDataToPc(float data1, float data2, float data3, float data4, float data5){
tvlogman 27:a4228ea8fb8f 58 // Capture data
tvlogman 27:a4228ea8fb8f 59 x = data1;
tvlogman 27:a4228ea8fb8f 60 y = data2;
tvlogman 27:a4228ea8fb8f 61 z = data3;
tvlogman 27:a4228ea8fb8f 62 q = data4;
tvlogman 27:a4228ea8fb8f 63 k = data5;
tvlogman 27:a4228ea8fb8f 64 scope.set(0, x);
tvlogman 27:a4228ea8fb8f 65 scope.set(1, y);
tvlogman 27:a4228ea8fb8f 66 scope.set(2, z);
tvlogman 27:a4228ea8fb8f 67 scope.set(3, q);
tvlogman 27:a4228ea8fb8f 68 scope.set(4, z);
tvlogman 27:a4228ea8fb8f 69 scope.send(); // send what's in scope memory to PC
tvlogman 27:a4228ea8fb8f 70 }
tvlogman 14:664870b5d153 71
tvlogman 7:1bffab95fc5f 72
tvlogman 33:6f4858b98fe5 73 // REFERENCE PARAMETERS
tvlogman 27:a4228ea8fb8f 74 int posRevRange = 2; // describes the ends of the position range in complete motor output shaft revolutions in both directions
tvlogman 21:d266d1e503ce 75 const float maxAngle = 2*3.14*posRevRange; // max angle in radians
tvlogman 22:2e473e9798c0 76 const float Ts = 0.1;
tvlogman 20:4ce3fb543a45 77
tvlogman 27:a4228ea8fb8f 78 // Function getReferencePosition returns reference angle based on potmeter 1
tvlogman 33:6f4858b98fe5 79 refGen ref1(A1, maxAngle);
tvlogman 33:6f4858b98fe5 80 refGen ref2(A1, maxAngle);
tvlogman 19:f08b5cd2b7ce 81
tvlogman 21:d266d1e503ce 82 // readEncoder reads counts and revs and logs results to serial window
tvlogman 33:6f4858b98fe5 83 errorFetch e1(ref1, gearRatio, Ts);
tvlogman 33:6f4858b98fe5 84 errorFetch e2(ref2, gearRatio, Ts);
tvlogman 21:d266d1e503ce 85
tvlogman 31:cc08254ab7b5 86 // Generate a PID controller with the specified values of k_p, k_d and k_i
tvlogman 30:65f0c9ecf810 87 controller motorController1(k_p, k_d, k_i);
tvlogman 14:664870b5d153 88
tvlogman 19:f08b5cd2b7ce 89 // setMotor1 sets the direction and magnitude pins of motor1 depending on the given motorValue. Negative motorValue means clockwise rotation
tvlogman 31:cc08254ab7b5 90 motorConfig motor1(LED_GREEN,LED_RED,LED_BLUE,D5,D4);
tvlogman 19:f08b5cd2b7ce 91
tvlogman 19:f08b5cd2b7ce 92 void measureAndControl(){
tvlogman 33:6f4858b98fe5 93 m1counts = Encoder1.getPulses();
tvlogman 33:6f4858b98fe5 94 m2counts = Encoder2.getPulses();
tvlogman 33:6f4858b98fe5 95 e1.fetchError(m1counts);
tvlogman 33:6f4858b98fe5 96 float motorValue = motorController1.control(e1.e_pos, e1.e_int, e1.e_der);
tvlogman 33:6f4858b98fe5 97 float r = ref1.getReferencePosition();
tvlogman 33:6f4858b98fe5 98 sendDataToPc(r, e1.e_pos, emg0.read(), e1.e_der, motorValue);
tvlogman 31:cc08254ab7b5 99 motor1.setMotor(motorValue);
tvlogman 15:b76b8cff4d8f 100 }
tvlogman 15:b76b8cff4d8f 101
tvlogman 27:a4228ea8fb8f 102 void rSwitchDirection(){
tvlogman 33:6f4858b98fe5 103 ref1.r_direction = !ref1.r_direction;
tvlogman 27:a4228ea8fb8f 104 pc.printf("Switched reference direction! \r\n");
tvlogman 14:664870b5d153 105 }
vsluiter 0:c8f15874531b 106
tvlogman 21:d266d1e503ce 107
vsluiter 0:c8f15874531b 108 int main()
tvlogman 10:e23cbcdde7e3 109 {
tvlogman 19:f08b5cd2b7ce 110 pc.printf("Main function");
tvlogman 31:cc08254ab7b5 111 sw2.fall(&motor1,&motorConfig::killSwitch);
tvlogman 31:cc08254ab7b5 112 sw3.fall(&motor1, &motorConfig::turnMotorOn);
tvlogman 27:a4228ea8fb8f 113 button2.rise(&rSwitchDirection);
vsluiter 0:c8f15874531b 114 pc.baud(115200);
tvlogman 22:2e473e9798c0 115 controllerTicker.attach(measureAndControl, Ts);
tvlogman 19:f08b5cd2b7ce 116 pc.printf("Encoder ticker attached and baudrate set");
vsluiter 0:c8f15874531b 117 }
tvlogman 7:1bffab95fc5f 118