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

Committer:
tvlogman
Date:
Fri Oct 06 07:23:26 2017 +0000
Revision:
20:4ce3fb543a45
Parent:
19:f08b5cd2b7ce
Child:
21:d266d1e503ce
Fixed the direction problems, tried, tested, works.

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 13:83e3672b24ee 14 DigitalOut motor1_direction(D4);
tvlogman 13:83e3672b24ee 15 PwmOut motor1_pwm(D5);
tvlogman 14:664870b5d153 16
tvlogman 14:664870b5d153 17 // Defining inputs
tvlogman 14:664870b5d153 18 InterruptIn sw2(SW2);
tvlogman 15:b76b8cff4d8f 19 InterruptIn sw3(SW3);
tvlogman 16:27430afe663e 20 InterruptIn button1(D2);
tvlogman 15:b76b8cff4d8f 21 AnalogIn pot(A0);
tvlogman 15:b76b8cff4d8f 22
tvlogman 16:27430afe663e 23 // PWM settings
tvlogman 16:27430afe663e 24 float pwmPeriod = 1.0/5000.0;
tvlogman 16:27430afe663e 25 int frequency_pwm = 10000; //10kHz PWM
tvlogman 14:664870b5d153 26
tvlogman 16:27430afe663e 27 volatile float x;
tvlogman 16:27430afe663e 28 volatile float x_prev =0;
tvlogman 16:27430afe663e 29 volatile float y; // filtered 'output' of ReadAnalogInAndFilter
tvlogman 14:664870b5d153 30
tvlogman 17:616ce7bc1f96 31 // Initializing encoder
tvlogman 10:e23cbcdde7e3 32 Ticker encoderTicker;
tvlogman 19:f08b5cd2b7ce 33 Ticker controllerTicker;
tvlogman 10:e23cbcdde7e3 34 volatile int counts = 0;
tvlogman 12:0462757e1ed2 35 volatile float revs = 0.00;
tvlogman 7:1bffab95fc5f 36
tvlogman 19:f08b5cd2b7ce 37 // MOTOR CONTROL PART
tvlogman 19:f08b5cd2b7ce 38
tvlogman 20:4ce3fb543a45 39 bool m1_direction = false;
tvlogman 20:4ce3fb543a45 40
tvlogman 19:f08b5cd2b7ce 41 // Function getReferenceVelocity returns reference r between -8.4 and 8.4 rad/s
tvlogman 19:f08b5cd2b7ce 42 float getReferenceVelocity(){
tvlogman 19:f08b5cd2b7ce 43 const float maxVelocity = 8.4; // max velocity in rad/s
tvlogman 19:f08b5cd2b7ce 44 double r;
tvlogman 20:4ce3fb543a45 45 if(m1_direction == false){
tvlogman 19:f08b5cd2b7ce 46 // Clockwise rotation yields positive reference
tvlogman 19:f08b5cd2b7ce 47 r = maxVelocity*pot.read();
tvlogman 19:f08b5cd2b7ce 48 }
tvlogman 20:4ce3fb543a45 49 if(m1_direction == true){
tvlogman 19:f08b5cd2b7ce 50 // Counterclockwise rotation yields negative reference
tvlogman 19:f08b5cd2b7ce 51 r = -1*maxVelocity*pot.read();
tvlogman 19:f08b5cd2b7ce 52 }
tvlogman 19:f08b5cd2b7ce 53 return r;
tvlogman 19:f08b5cd2b7ce 54 }
tvlogman 19:f08b5cd2b7ce 55
tvlogman 19:f08b5cd2b7ce 56 // motorController sets a motorValue based on the reference. Sign indicates direction and magnitude indicates speed. Right now this is a simple feedforward
tvlogman 19:f08b5cd2b7ce 57 float motorController(float reference){
tvlogman 19:f08b5cd2b7ce 58 const float motorGain = 8.4; // max power of the motor should normalize to a motorValue magnitude of 1
tvlogman 19:f08b5cd2b7ce 59 double motorValue = reference/motorGain; // motorValue is the normalized voltage applied to motor magnitude pin between -1 and 1
tvlogman 19:f08b5cd2b7ce 60 return motorValue;
tvlogman 10:e23cbcdde7e3 61 }
tvlogman 14:664870b5d153 62
tvlogman 19:f08b5cd2b7ce 63 // setMotor1 sets the direction and magnitude pins of motor1 depending on the given motorValue. Negative motorValue means clockwise rotation
tvlogman 19:f08b5cd2b7ce 64 void setMotor1(float motorValue){
tvlogman 19:f08b5cd2b7ce 65 switch(currentState){
tvlogman 19:f08b5cd2b7ce 66 case KILLED:
tvlogman 19:f08b5cd2b7ce 67 motor1_pwm.write(0.0);
tvlogman 19:f08b5cd2b7ce 68 break;
tvlogman 19:f08b5cd2b7ce 69 case ACTIVE:
tvlogman 19:f08b5cd2b7ce 70 // Set motor direction
tvlogman 19:f08b5cd2b7ce 71 if (motorValue >=0){
tvlogman 19:f08b5cd2b7ce 72 motor1_direction = 0;
tvlogman 19:f08b5cd2b7ce 73 }
tvlogman 19:f08b5cd2b7ce 74 else {
tvlogman 19:f08b5cd2b7ce 75 motor1_direction = 1;
tvlogman 19:f08b5cd2b7ce 76 }
tvlogman 19:f08b5cd2b7ce 77 // Set motor speed
tvlogman 19:f08b5cd2b7ce 78 if (fabs(motorValue)>1){
tvlogman 19:f08b5cd2b7ce 79 motor1_pwm = 1;
tvlogman 19:f08b5cd2b7ce 80 }
tvlogman 19:f08b5cd2b7ce 81 else {
tvlogman 19:f08b5cd2b7ce 82 motor1_pwm.write(fabs(motorValue));
tvlogman 19:f08b5cd2b7ce 83 }
tvlogman 19:f08b5cd2b7ce 84 break;
tvlogman 19:f08b5cd2b7ce 85 }
tvlogman 19:f08b5cd2b7ce 86 }
tvlogman 19:f08b5cd2b7ce 87
tvlogman 19:f08b5cd2b7ce 88 void measureAndControl(){
tvlogman 19:f08b5cd2b7ce 89 float referenceVelocity = getReferenceVelocity();
tvlogman 19:f08b5cd2b7ce 90 float motorValue = motorController(referenceVelocity);
tvlogman 19:f08b5cd2b7ce 91 pc.printf("Motorvalue is %.2f \r \n", motorValue);
tvlogman 19:f08b5cd2b7ce 92 pc.printf("Motor direction is %d \r \n", motor1_direction);
tvlogman 19:f08b5cd2b7ce 93 setMotor1(motorValue);
tvlogman 19:f08b5cd2b7ce 94 }
tvlogman 19:f08b5cd2b7ce 95
tvlogman 14:664870b5d153 96 void killSwitch(){
tvlogman 15:b76b8cff4d8f 97 pc.printf("Motors turned off");
tvlogman 15:b76b8cff4d8f 98 currentState = KILLED;
tvlogman 14:664870b5d153 99 }
tvlogman 14:664870b5d153 100
tvlogman 15:b76b8cff4d8f 101 void turnMotorsOn(){
tvlogman 15:b76b8cff4d8f 102 pc.printf("Motors turned on");
tvlogman 15:b76b8cff4d8f 103 currentState = ACTIVE;
tvlogman 15:b76b8cff4d8f 104 }
tvlogman 15:b76b8cff4d8f 105
tvlogman 15:b76b8cff4d8f 106
tvlogman 14:664870b5d153 107 void M1switchDirection(){
tvlogman 20:4ce3fb543a45 108 m1_direction = !m1_direction;
tvlogman 14:664870b5d153 109 }
vsluiter 0:c8f15874531b 110
tvlogman 19:f08b5cd2b7ce 111 // readEncoder reads counts and revs and logs results to serial window
tvlogman 19:f08b5cd2b7ce 112 void readEncoder(){
tvlogman 19:f08b5cd2b7ce 113 counts = Encoder.getPulses();
tvlogman 19:f08b5cd2b7ce 114 revs = counts/(131*64.0f);
tvlogman 19:f08b5cd2b7ce 115 pc.printf("%0.2f revolutions \r\n", revs);
tvlogman 19:f08b5cd2b7ce 116 float reference = getReferenceVelocity();
tvlogman 19:f08b5cd2b7ce 117 pc.printf("Reference velocity %0.2f \r\n", reference);
tvlogman 19:f08b5cd2b7ce 118 }
tvlogman 19:f08b5cd2b7ce 119
vsluiter 0:c8f15874531b 120 int main()
tvlogman 10:e23cbcdde7e3 121 {
tvlogman 19:f08b5cd2b7ce 122 pc.printf("Main function");
tvlogman 20:4ce3fb543a45 123 motor1_direction = 0; // False = clockwise rotation
tvlogman 13:83e3672b24ee 124 motor1_pwm.period(pwmPeriod);//T=1/f
tvlogman 14:664870b5d153 125 sw2.fall(&killSwitch);
tvlogman 15:b76b8cff4d8f 126 sw3.fall(&turnMotorsOn);
tvlogman 16:27430afe663e 127 button1.rise(&M1switchDirection);
vsluiter 0:c8f15874531b 128 pc.baud(115200);
tvlogman 19:f08b5cd2b7ce 129 encoderTicker.attach(readEncoder, 2);
tvlogman 19:f08b5cd2b7ce 130 controllerTicker.attach(measureAndControl, 0.1);
tvlogman 15:b76b8cff4d8f 131
tvlogman 19:f08b5cd2b7ce 132 pc.printf("Encoder ticker attached and baudrate set");
vsluiter 0:c8f15874531b 133 }
tvlogman 7:1bffab95fc5f 134