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@26:4f84448b4d46, 2017-10-06 (annotated)

Committer:
tvlogman
Date:
Fri Oct 06 13:11:24 2017 +0000
Revision:
26:4f84448b4d46
Parent:
25:df780572cfc8
Child:
27:a4228ea8fb8f
All the individual parts seem to be working, never gets to a total error of 0 though. Why?

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 15:b76b8cff4d8f 6
tvlogman 15:b76b8cff4d8f 7 enum robotStates {KILLED, ACTIVE};
tvlogman 15:b76b8cff4d8f 8 robotStates currentState = KILLED;
tvlogman 8:0067469c3389 9
tvlogman 12:0462757e1ed2 10 QEI Encoder(D12,D13,NC,64, QEI::X4_ENCODING);
tvlogman 10:e23cbcdde7e3 11 MODSERIAL pc(USBTX, USBRX);
tvlogman 8:0067469c3389 12
tvlogman 14:664870b5d153 13 // Defining outputs
tvlogman 22:2e473e9798c0 14
tvlogman 22:2e473e9798c0 15 // Leds can be used to indicate status
tvlogman 22:2e473e9798c0 16 DigitalOut ledG(LED_GREEN);
tvlogman 22:2e473e9798c0 17 DigitalOut ledR(LED_RED);
tvlogman 22:2e473e9798c0 18 DigitalOut ledB(LED_BLUE);
tvlogman 22:2e473e9798c0 19
tvlogman 13:83e3672b24ee 20 DigitalOut motor1_direction(D4);
tvlogman 13:83e3672b24ee 21 PwmOut motor1_pwm(D5);
tvlogman 14:664870b5d153 22
tvlogman 14:664870b5d153 23 // Defining inputs
tvlogman 14:664870b5d153 24 InterruptIn sw2(SW2);
tvlogman 15:b76b8cff4d8f 25 InterruptIn sw3(SW3);
tvlogman 16:27430afe663e 26 InterruptIn button1(D2);
tvlogman 21:d266d1e503ce 27 AnalogIn pot1(A0);
tvlogman 21:d266d1e503ce 28 AnalogIn pot2(A1);
tvlogman 15:b76b8cff4d8f 29
tvlogman 16:27430afe663e 30 // PWM settings
tvlogman 16:27430afe663e 31 float pwmPeriod = 1.0/5000.0;
tvlogman 16:27430afe663e 32 int frequency_pwm = 10000; //10kHz PWM
tvlogman 14:664870b5d153 33
tvlogman 16:27430afe663e 34 volatile float x;
tvlogman 16:27430afe663e 35 volatile float x_prev =0;
tvlogman 16:27430afe663e 36 volatile float y; // filtered 'output' of ReadAnalogInAndFilter
tvlogman 14:664870b5d153 37
tvlogman 17:616ce7bc1f96 38 // Initializing encoder
tvlogman 10:e23cbcdde7e3 39 Ticker encoderTicker;
tvlogman 19:f08b5cd2b7ce 40 Ticker controllerTicker;
tvlogman 10:e23cbcdde7e3 41 volatile int counts = 0;
tvlogman 12:0462757e1ed2 42 volatile float revs = 0.00;
tvlogman 7:1bffab95fc5f 43
tvlogman 19:f08b5cd2b7ce 44 // MOTOR CONTROL PART
tvlogman 20:4ce3fb543a45 45 bool m1_direction = false;
tvlogman 24:672abc3f02b7 46 int posRevRange = 5; // describes the ends of the position range in complete motor output shaft revolutions in both directions
tvlogman 21:d266d1e503ce 47 const float maxAngle = 2*3.14*posRevRange; // max angle in radians
tvlogman 22:2e473e9798c0 48 const float Ts = 0.1;
tvlogman 20:4ce3fb543a45 49
tvlogman 21:d266d1e503ce 50 // Function getReferencePosition returns reference angle within range
tvlogman 21:d266d1e503ce 51 float getReferencePosition(){
tvlogman 19:f08b5cd2b7ce 52 double r;
tvlogman 20:4ce3fb543a45 53 if(m1_direction == false){
tvlogman 19:f08b5cd2b7ce 54 // Clockwise rotation yields positive reference
tvlogman 21:d266d1e503ce 55 r = maxAngle*pot1.read();
tvlogman 19:f08b5cd2b7ce 56 }
tvlogman 20:4ce3fb543a45 57 if(m1_direction == true){
tvlogman 19:f08b5cd2b7ce 58 // Counterclockwise rotation yields negative reference
tvlogman 21:d266d1e503ce 59 r = -1*maxAngle*pot1.read();
tvlogman 19:f08b5cd2b7ce 60 }
tvlogman 19:f08b5cd2b7ce 61 return r;
tvlogman 19:f08b5cd2b7ce 62 }
tvlogman 19:f08b5cd2b7ce 63
tvlogman 22:2e473e9798c0 64 // Initializing position and integral errors
tvlogman 25:df780572cfc8 65 float e_pos = 0;
tvlogman 25:df780572cfc8 66 float e_int = 0;
tvlogman 25:df780572cfc8 67 float e_der = 0;
tvlogman 24:672abc3f02b7 68 float e_prev = 0;
tvlogman 25:df780572cfc8 69 float e = e_pos + e_int + e_der;
tvlogman 22:2e473e9798c0 70
tvlogman 21:d266d1e503ce 71 // readEncoder reads counts and revs and logs results to serial window
tvlogman 25:df780572cfc8 72 void getError(float &e_pos, float &e_int, float &e_der){
tvlogman 21:d266d1e503ce 73 counts = Encoder.getPulses();
tvlogman 21:d266d1e503ce 74 double motor1Position = 2*3.14*(counts/(131*64.0f));
tvlogman 21:d266d1e503ce 75 pc.printf("%0.2f revolutions \r\n", motor1Position);
tvlogman 25:df780572cfc8 76 e_pos = getReferencePosition() - motor1Position;
tvlogman 25:df780572cfc8 77 e_int = e_int + Ts*e_pos;
tvlogman 26:4f84448b4d46 78 e_der = (e_pos - e_prev)/Ts;
tvlogman 26:4f84448b4d46 79 e = e_pos + e_int + e_der; // not sure if this is really even all that useful
tvlogman 26:4f84448b4d46 80 e_prev = e_pos; // Store current error as we'll need it to compute the next derivative error
tvlogman 25:df780572cfc8 81 pc.printf("Error is %0.2f \r \n", e);
tvlogman 21:d266d1e503ce 82 }
tvlogman 21:d266d1e503ce 83
tvlogman 26:4f84448b4d46 84 const float k_p = 0.1; // use potmeter 2 to adjust k_p within range 0 to 1
tvlogman 26:4f84448b4d46 85 const float k_i = 0.05; // Still needs a reasonable value
tvlogman 26:4f84448b4d46 86 const float k_d = 0.01; // Again, still need to pick a reasonable value
tvlogman 26:4f84448b4d46 87
tvlogman 21:d266d1e503ce 88 // motorController sets a motorValue based on the position error. Sign indicates direction and magnitude indicates speed. Right now this is a simple feedforward
tvlogman 26:4f84448b4d46 89 float motorController(float e_pos, float e_int, float e_der){
tvlogman 26:4f84448b4d46 90 // NOTE: do I still need the maxangle bit here?
tvlogman 26:4f84448b4d46 91 double motorValue = (e_pos*k_p) + 0.35 + k_i*e_int + k_d*e_der; // motorValue is the normalized voltage applied to motor magnitude pin between -1 and 1 added 0.35 as motorvalue of 0.35 does nothing
tvlogman 19:f08b5cd2b7ce 92 return motorValue;
tvlogman 10:e23cbcdde7e3 93 }
tvlogman 14:664870b5d153 94
tvlogman 19:f08b5cd2b7ce 95 // setMotor1 sets the direction and magnitude pins of motor1 depending on the given motorValue. Negative motorValue means clockwise rotation
tvlogman 19:f08b5cd2b7ce 96 void setMotor1(float motorValue){
tvlogman 19:f08b5cd2b7ce 97 switch(currentState){
tvlogman 19:f08b5cd2b7ce 98 case KILLED:
tvlogman 19:f08b5cd2b7ce 99 motor1_pwm.write(0.0);
tvlogman 22:2e473e9798c0 100 ledR = 0;
tvlogman 22:2e473e9798c0 101 ledG = 1;
tvlogman 22:2e473e9798c0 102 ledB = 1;
tvlogman 19:f08b5cd2b7ce 103 break;
tvlogman 19:f08b5cd2b7ce 104 case ACTIVE:
tvlogman 19:f08b5cd2b7ce 105 // Set motor direction
tvlogman 19:f08b5cd2b7ce 106 if (motorValue >=0){
tvlogman 19:f08b5cd2b7ce 107 motor1_direction = 0;
tvlogman 19:f08b5cd2b7ce 108 }
tvlogman 19:f08b5cd2b7ce 109 else {
tvlogman 19:f08b5cd2b7ce 110 motor1_direction = 1;
tvlogman 19:f08b5cd2b7ce 111 }
tvlogman 19:f08b5cd2b7ce 112 // Set motor speed
tvlogman 19:f08b5cd2b7ce 113 if (fabs(motorValue)>1){
tvlogman 19:f08b5cd2b7ce 114 motor1_pwm = 1;
tvlogman 19:f08b5cd2b7ce 115 }
tvlogman 19:f08b5cd2b7ce 116 else {
tvlogman 19:f08b5cd2b7ce 117 motor1_pwm.write(fabs(motorValue));
tvlogman 22:2e473e9798c0 118 }
tvlogman 22:2e473e9798c0 119 ledR = 1;
tvlogman 22:2e473e9798c0 120 ledG = 1;
tvlogman 22:2e473e9798c0 121 ledB = 0;
tvlogman 19:f08b5cd2b7ce 122 break;
tvlogman 19:f08b5cd2b7ce 123 }
tvlogman 19:f08b5cd2b7ce 124 }
tvlogman 19:f08b5cd2b7ce 125
tvlogman 19:f08b5cd2b7ce 126 void measureAndControl(){
tvlogman 26:4f84448b4d46 127 getError(e_pos, e_int, e_der);
tvlogman 26:4f84448b4d46 128 float motorValue = motorController(e_pos, e_int, e_der);
tvlogman 26:4f84448b4d46 129 pc.printf("Position action is %.2f \r \n", k_p*e_pos);
tvlogman 26:4f84448b4d46 130 pc.printf("Derivative action is %.2f \r \n", k_d*e_der);
tvlogman 26:4f84448b4d46 131 pc.printf("Integral action is %.2f", k_i*e_int);
tvlogman 26:4f84448b4d46 132 pc.printf("Total action is %.2f", k_p*e_pos + k_d*e_der + k_i*e_int);
tvlogman 26:4f84448b4d46 133 pc.printf("Motorvalue is %.2f \r \n", motorValue);
tvlogman 26:4f84448b4d46 134 pc.printf("Actual error is %.2f \r \n", e_pos);
tvlogman 19:f08b5cd2b7ce 135 setMotor1(motorValue);
tvlogman 19:f08b5cd2b7ce 136 }
tvlogman 19:f08b5cd2b7ce 137
tvlogman 14:664870b5d153 138 void killSwitch(){
tvlogman 15:b76b8cff4d8f 139 pc.printf("Motors turned off");
tvlogman 15:b76b8cff4d8f 140 currentState = KILLED;
tvlogman 14:664870b5d153 141 }
tvlogman 14:664870b5d153 142
tvlogman 15:b76b8cff4d8f 143 void turnMotorsOn(){
tvlogman 15:b76b8cff4d8f 144 pc.printf("Motors turned on");
tvlogman 15:b76b8cff4d8f 145 currentState = ACTIVE;
tvlogman 15:b76b8cff4d8f 146 }
tvlogman 15:b76b8cff4d8f 147
tvlogman 15:b76b8cff4d8f 148
tvlogman 14:664870b5d153 149 void M1switchDirection(){
tvlogman 20:4ce3fb543a45 150 m1_direction = !m1_direction;
tvlogman 22:2e473e9798c0 151 pc.printf("Switched direction!");
tvlogman 14:664870b5d153 152 }
vsluiter 0:c8f15874531b 153
tvlogman 21:d266d1e503ce 154
tvlogman 19:f08b5cd2b7ce 155
vsluiter 0:c8f15874531b 156 int main()
tvlogman 10:e23cbcdde7e3 157 {
tvlogman 19:f08b5cd2b7ce 158 pc.printf("Main function");
tvlogman 20:4ce3fb543a45 159 motor1_direction = 0; // False = clockwise rotation
tvlogman 13:83e3672b24ee 160 motor1_pwm.period(pwmPeriod);//T=1/f
tvlogman 14:664870b5d153 161 sw2.fall(&killSwitch);
tvlogman 15:b76b8cff4d8f 162 sw3.fall(&turnMotorsOn);
tvlogman 16:27430afe663e 163 button1.rise(&M1switchDirection);
vsluiter 0:c8f15874531b 164 pc.baud(115200);
tvlogman 22:2e473e9798c0 165 controllerTicker.attach(measureAndControl, Ts);
tvlogman 15:b76b8cff4d8f 166
tvlogman 19:f08b5cd2b7ce 167 pc.printf("Encoder ticker attached and baudrate set");
vsluiter 0:c8f15874531b 168 }
tvlogman 7:1bffab95fc5f 169