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@32:1bb406d2b3c3, 2017-10-20 (annotated)

Committer:
tvlogman
Date:
Fri Oct 20 12:24:37 2017 +0000
Revision:
32:1bb406d2b3c3
Parent:
31:cc08254ab7b5
Child:
33:6f4858b98fe5
Broke it... gets strange error saying that class constructor for "Noncopyable" class is inaccessible.;

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