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@23:917179f72762, 2017-10-06 (annotated)

Committer:
tvlogman
Date:
Fri Oct 06 12:03:26 2017 +0000
Revision:
23:917179f72762
Parent:
22:2e473e9798c0
Child:
24:672abc3f02b7
Fixed the I-action bit: now I action is based on the position error multiplied by Ts added to the previous I error instead of the Ts multiplied by total I error.

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 23:917179f72762 46 int posRevRange = 2; // 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 22:2e473e9798c0 65 float posError = 0;
tvlogman 22:2e473e9798c0 66 float integralError = 0;
tvlogman 22:2e473e9798c0 67 float totalError = posError + integralError;
tvlogman 22:2e473e9798c0 68
tvlogman 21:d266d1e503ce 69 // readEncoder reads counts and revs and logs results to serial window
tvlogman 22:2e473e9798c0 70 void getError(float &posError, float &integralError){
tvlogman 21:d266d1e503ce 71 counts = Encoder.getPulses();
tvlogman 21:d266d1e503ce 72 double motor1Position = 2*3.14*(counts/(131*64.0f));
tvlogman 21:d266d1e503ce 73 pc.printf("%0.2f revolutions \r\n", motor1Position);
tvlogman 22:2e473e9798c0 74 posError = getReferencePosition() - motor1Position;
tvlogman 22:2e473e9798c0 75
tvlogman 23:917179f72762 76 integralError = integralError + Ts*posError;
tvlogman 22:2e473e9798c0 77 totalError = posError + integralError;
tvlogman 22:2e473e9798c0 78 pc.printf("Error is %0.2f \r \n", totalError);
tvlogman 21:d266d1e503ce 79 }
tvlogman 21:d266d1e503ce 80
tvlogman 21:d266d1e503ce 81 // motorController sets a motorValue based on the position error. Sign indicates direction and magnitude indicates speed. Right now this is a simple feedforward
tvlogman 22:2e473e9798c0 82 float motorController(float posError, float integralError){
tvlogman 21:d266d1e503ce 83 float k_p = 5*pot2.read(); // use potmeter 2 to adjust k_p within range 0 to 1
tvlogman 23:917179f72762 84 float k_i = 0.001; // How do I pick a reasonable value for k_i?
tvlogman 22:2e473e9798c0 85 double motorValue = (posError*k_p)/maxAngle + 0.35 + k_i*integralError; // 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 86 return motorValue;
tvlogman 10:e23cbcdde7e3 87 }
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 19:f08b5cd2b7ce 90 void setMotor1(float motorValue){
tvlogman 19:f08b5cd2b7ce 91 switch(currentState){
tvlogman 19:f08b5cd2b7ce 92 case KILLED:
tvlogman 19:f08b5cd2b7ce 93 motor1_pwm.write(0.0);
tvlogman 22:2e473e9798c0 94 ledR = 0;
tvlogman 22:2e473e9798c0 95 ledG = 1;
tvlogman 22:2e473e9798c0 96 ledB = 1;
tvlogman 19:f08b5cd2b7ce 97 break;
tvlogman 19:f08b5cd2b7ce 98 case ACTIVE:
tvlogman 19:f08b5cd2b7ce 99 // Set motor direction
tvlogman 19:f08b5cd2b7ce 100 if (motorValue >=0){
tvlogman 19:f08b5cd2b7ce 101 motor1_direction = 0;
tvlogman 19:f08b5cd2b7ce 102 }
tvlogman 19:f08b5cd2b7ce 103 else {
tvlogman 19:f08b5cd2b7ce 104 motor1_direction = 1;
tvlogman 19:f08b5cd2b7ce 105 }
tvlogman 19:f08b5cd2b7ce 106 // Set motor speed
tvlogman 19:f08b5cd2b7ce 107 if (fabs(motorValue)>1){
tvlogman 19:f08b5cd2b7ce 108 motor1_pwm = 1;
tvlogman 19:f08b5cd2b7ce 109 }
tvlogman 19:f08b5cd2b7ce 110 else {
tvlogman 19:f08b5cd2b7ce 111 motor1_pwm.write(fabs(motorValue));
tvlogman 22:2e473e9798c0 112 }
tvlogman 22:2e473e9798c0 113 ledR = 1;
tvlogman 22:2e473e9798c0 114 ledG = 1;
tvlogman 22:2e473e9798c0 115 ledB = 0;
tvlogman 19:f08b5cd2b7ce 116 break;
tvlogman 19:f08b5cd2b7ce 117 }
tvlogman 19:f08b5cd2b7ce 118 }
tvlogman 19:f08b5cd2b7ce 119
tvlogman 19:f08b5cd2b7ce 120 void measureAndControl(){
tvlogman 22:2e473e9798c0 121 getError(posError, integralError);
tvlogman 22:2e473e9798c0 122 float motorValue = motorController(posError, integralError);
tvlogman 21:d266d1e503ce 123 pc.printf("Position error is %.2f \r \n", posError);
tvlogman 22:2e473e9798c0 124 pc.printf("Total error is %.2f \r \n", totalError);
tvlogman 22:2e473e9798c0 125 pc.printf("Integral error is %2f", integralError);
tvlogman 22:2e473e9798c0 126 //pc.printf("Motorvalue is %.2f \r \n", motorValue);
tvlogman 22:2e473e9798c0 127 //pc.printf("Motor direction is %d \r \n", motor1_direction);
tvlogman 19:f08b5cd2b7ce 128 setMotor1(motorValue);
tvlogman 19:f08b5cd2b7ce 129 }
tvlogman 19:f08b5cd2b7ce 130
tvlogman 14:664870b5d153 131 void killSwitch(){
tvlogman 15:b76b8cff4d8f 132 pc.printf("Motors turned off");
tvlogman 15:b76b8cff4d8f 133 currentState = KILLED;
tvlogman 14:664870b5d153 134 }
tvlogman 14:664870b5d153 135
tvlogman 15:b76b8cff4d8f 136 void turnMotorsOn(){
tvlogman 15:b76b8cff4d8f 137 pc.printf("Motors turned on");
tvlogman 15:b76b8cff4d8f 138 currentState = ACTIVE;
tvlogman 15:b76b8cff4d8f 139 }
tvlogman 15:b76b8cff4d8f 140
tvlogman 15:b76b8cff4d8f 141
tvlogman 14:664870b5d153 142 void M1switchDirection(){
tvlogman 20:4ce3fb543a45 143 m1_direction = !m1_direction;
tvlogman 22:2e473e9798c0 144 pc.printf("Switched direction!");
tvlogman 14:664870b5d153 145 }
vsluiter 0:c8f15874531b 146
tvlogman 21:d266d1e503ce 147
tvlogman 19:f08b5cd2b7ce 148
vsluiter 0:c8f15874531b 149 int main()
tvlogman 10:e23cbcdde7e3 150 {
tvlogman 19:f08b5cd2b7ce 151 pc.printf("Main function");
tvlogman 20:4ce3fb543a45 152 motor1_direction = 0; // False = clockwise rotation
tvlogman 13:83e3672b24ee 153 motor1_pwm.period(pwmPeriod);//T=1/f
tvlogman 14:664870b5d153 154 sw2.fall(&killSwitch);
tvlogman 15:b76b8cff4d8f 155 sw3.fall(&turnMotorsOn);
tvlogman 16:27430afe663e 156 button1.rise(&M1switchDirection);
vsluiter 0:c8f15874531b 157 pc.baud(115200);
tvlogman 22:2e473e9798c0 158 controllerTicker.attach(measureAndControl, Ts);
tvlogman 15:b76b8cff4d8f 159
tvlogman 19:f08b5cd2b7ce 160 pc.printf("Encoder ticker attached and baudrate set");
vsluiter 0:c8f15874531b 161 }
tvlogman 7:1bffab95fc5f 162