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@16:27430afe663e, 2017-09-21 (annotated)

Committer:
tvlogman
Date:
Thu Sep 21 11:20:03 2017 +0000
Revision:
16:27430afe663e
Parent:
15:b76b8cff4d8f
Child:
17:616ce7bc1f96
Added a reverse-direction button on button1 of the Biorobotics shield

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 16:27430afe663e 12 HIDScope scope(2);
tvlogman 8:0067469c3389 13
tvlogman 14:664870b5d153 14 // Defining outputs
tvlogman 13:83e3672b24ee 15 DigitalOut motor1_direction(D4);
tvlogman 13:83e3672b24ee 16 PwmOut motor1_pwm(D5);
tvlogman 14:664870b5d153 17
tvlogman 14:664870b5d153 18 // Defining inputs
tvlogman 14:664870b5d153 19 InterruptIn sw2(SW2);
tvlogman 15:b76b8cff4d8f 20 InterruptIn sw3(SW3);
tvlogman 16:27430afe663e 21 InterruptIn button1(D2);
tvlogman 15:b76b8cff4d8f 22 AnalogIn pot(A0);
tvlogman 15:b76b8cff4d8f 23
tvlogman 16:27430afe663e 24 // PWM settings
tvlogman 16:27430afe663e 25 float pwmPeriod = 1.0/5000.0;
tvlogman 16:27430afe663e 26 int frequency_pwm = 10000; //10kHz PWM
tvlogman 14:664870b5d153 27
tvlogman 16:27430afe663e 28 volatile float x;
tvlogman 16:27430afe663e 29 volatile float x_prev =0;
tvlogman 16:27430afe663e 30 volatile float y; // filtered 'output' of ReadAnalogInAndFilter
tvlogman 16:27430afe663e 31 volatile float ledBrightness = 0.00;
tvlogman 14:664870b5d153 32
tvlogman 13:83e3672b24ee 33
tvlogman 10:e23cbcdde7e3 34 Ticker encoderTicker;
tvlogman 10:e23cbcdde7e3 35 volatile int counts = 0;
tvlogman 12:0462757e1ed2 36 volatile float revs = 0.00;
tvlogman 7:1bffab95fc5f 37
tvlogman 16:27430afe663e 38
tvlogman 16:27430afe663e 39
tvlogman 10:e23cbcdde7e3 40 void readEncoder(){
tvlogman 10:e23cbcdde7e3 41 counts = Encoder.getPulses();
tvlogman 12:0462757e1ed2 42 revs = counts/64.0f;
tvlogman 16:27430afe663e 43
tvlogman 16:27430afe663e 44 // Displaying revs in HIDscope
tvlogman 16:27430afe663e 45 x = revs; // Capture data
tvlogman 16:27430afe663e 46 scope.set(0, x); // store data in first element of scope memory
tvlogman 16:27430afe663e 47 y = (x_prev + x)/2.0; // averaging filter
tvlogman 16:27430afe663e 48 scope.set(1, y); // store data in second element of scope memory
tvlogman 16:27430afe663e 49 x_prev = x; // Prepare for next round
tvlogman 16:27430afe663e 50
tvlogman 16:27430afe663e 51 scope.send(); // send what's in scope memory to PC
tvlogman 10:e23cbcdde7e3 52 }
tvlogman 14:664870b5d153 53
tvlogman 14:664870b5d153 54 void killSwitch(){
tvlogman 15:b76b8cff4d8f 55 pc.printf("Motors turned off");
tvlogman 15:b76b8cff4d8f 56 currentState = KILLED;
tvlogman 14:664870b5d153 57 }
tvlogman 14:664870b5d153 58
tvlogman 15:b76b8cff4d8f 59 void turnMotorsOn(){
tvlogman 15:b76b8cff4d8f 60 pc.printf("Motors turned on");
tvlogman 15:b76b8cff4d8f 61 currentState = ACTIVE;
tvlogman 15:b76b8cff4d8f 62 }
tvlogman 15:b76b8cff4d8f 63
tvlogman 15:b76b8cff4d8f 64
tvlogman 14:664870b5d153 65 void M1switchDirection(){
tvlogman 14:664870b5d153 66 motor1_direction = !motor1_direction;
tvlogman 14:664870b5d153 67 }
vsluiter 0:c8f15874531b 68
vsluiter 0:c8f15874531b 69 int main()
tvlogman 10:e23cbcdde7e3 70 {
tvlogman 14:664870b5d153 71 motor1_direction = false;
tvlogman 13:83e3672b24ee 72 motor1_pwm.period(pwmPeriod);//T=1/f
tvlogman 14:664870b5d153 73 sw2.fall(&killSwitch);
tvlogman 15:b76b8cff4d8f 74 sw3.fall(&turnMotorsOn);
tvlogman 16:27430afe663e 75 button1.rise(&M1switchDirection);
vsluiter 0:c8f15874531b 76 pc.baud(115200);
tvlogman 16:27430afe663e 77 encoderTicker.attach(readEncoder, 0.1);
tvlogman 15:b76b8cff4d8f 78
tvlogman 10:e23cbcdde7e3 79 pc.printf("Encoder ticker attached and baudrate set");
tvlogman 13:83e3672b24ee 80
tvlogman 15:b76b8cff4d8f 81 while(true){
tvlogman 15:b76b8cff4d8f 82 switch(currentState){
tvlogman 15:b76b8cff4d8f 83 case KILLED:
tvlogman 15:b76b8cff4d8f 84 motor1_pwm.write(0.0);
tvlogman 15:b76b8cff4d8f 85 break;
tvlogman 15:b76b8cff4d8f 86 case ACTIVE:
tvlogman 15:b76b8cff4d8f 87 motor1_pwm.write(pot.read()); // Motor speed proportional to pot meter
tvlogman 15:b76b8cff4d8f 88 break;
tvlogman 15:b76b8cff4d8f 89 }
tvlogman 15:b76b8cff4d8f 90 }
tvlogman 15:b76b8cff4d8f 91
vsluiter 0:c8f15874531b 92 }
tvlogman 7:1bffab95fc5f 93