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@15:b76b8cff4d8f, 2017-09-21 (annotated)

Committer:
tvlogman
Date:
Thu Sep 21 11:00:55 2017 +0000
Revision:
15:b76b8cff4d8f
Parent:
14:664870b5d153
Child:
16:27430afe663e
Changed control scheme to finite state machine

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 15:b76b8cff4d8f 20 AnalogIn pot(A0);
tvlogman 15:b76b8cff4d8f 21
tvlogman 15:b76b8cff4d8f 22 // Enabling motors
tvlogman 15:b76b8cff4d8f 23 bool motorsOn = true;
tvlogman 14:664870b5d153 24
tvlogman 14:664870b5d153 25
tvlogman 13:83e3672b24ee 26 float pwmPeriod = 1.0/5000.0;
tvlogman 13:83e3672b24ee 27
tvlogman 10:e23cbcdde7e3 28 Ticker encoderTicker;
tvlogman 10:e23cbcdde7e3 29 volatile int counts = 0;
tvlogman 12:0462757e1ed2 30 volatile float revs = 0.00;
tvlogman 7:1bffab95fc5f 31
tvlogman 10:e23cbcdde7e3 32 void readEncoder(){
tvlogman 10:e23cbcdde7e3 33 counts = Encoder.getPulses();
tvlogman 12:0462757e1ed2 34 revs = counts/64.0f;
tvlogman 12:0462757e1ed2 35 pc.printf("%i pulses \r\n", counts);
tvlogman 12:0462757e1ed2 36 pc.printf("%f revolutions \r\n", revs);
tvlogman 10:e23cbcdde7e3 37 }
tvlogman 14:664870b5d153 38
tvlogman 14:664870b5d153 39 void killSwitch(){
tvlogman 15:b76b8cff4d8f 40 pc.printf("Motors turned off");
tvlogman 15:b76b8cff4d8f 41 currentState = KILLED;
tvlogman 14:664870b5d153 42 }
tvlogman 14:664870b5d153 43
tvlogman 15:b76b8cff4d8f 44 void turnMotorsOn(){
tvlogman 15:b76b8cff4d8f 45 pc.printf("Motors turned on");
tvlogman 15:b76b8cff4d8f 46 currentState = ACTIVE;
tvlogman 15:b76b8cff4d8f 47 }
tvlogman 15:b76b8cff4d8f 48
tvlogman 15:b76b8cff4d8f 49
tvlogman 14:664870b5d153 50 void M1switchDirection(){
tvlogman 14:664870b5d153 51 motor1_direction = !motor1_direction;
tvlogman 14:664870b5d153 52 }
vsluiter 0:c8f15874531b 53
tvlogman 13:83e3672b24ee 54 int frequency_pwm = 10000; //10kHz PWM
tvlogman 13:83e3672b24ee 55
vsluiter 0:c8f15874531b 56 int main()
tvlogman 10:e23cbcdde7e3 57 {
tvlogman 14:664870b5d153 58 motor1_direction = false;
tvlogman 13:83e3672b24ee 59 motor1_pwm.period(pwmPeriod);//T=1/f
tvlogman 14:664870b5d153 60 sw2.fall(&killSwitch);
tvlogman 15:b76b8cff4d8f 61 sw3.fall(&turnMotorsOn);
vsluiter 0:c8f15874531b 62 pc.baud(115200);
tvlogman 15:b76b8cff4d8f 63 encoderTicker.attach(readEncoder, 1.0);
tvlogman 15:b76b8cff4d8f 64
tvlogman 10:e23cbcdde7e3 65 pc.printf("Encoder ticker attached and baudrate set");
tvlogman 13:83e3672b24ee 66
tvlogman 15:b76b8cff4d8f 67 while(true){
tvlogman 15:b76b8cff4d8f 68 switch(currentState){
tvlogman 15:b76b8cff4d8f 69 case KILLED:
tvlogman 15:b76b8cff4d8f 70 motor1_pwm.write(0.0);
tvlogman 15:b76b8cff4d8f 71 break;
tvlogman 15:b76b8cff4d8f 72 case ACTIVE:
tvlogman 15:b76b8cff4d8f 73 motor1_pwm.write(pot.read()); // Motor speed proportional to pot meter
tvlogman 15:b76b8cff4d8f 74 break;
tvlogman 15:b76b8cff4d8f 75 }
tvlogman 15:b76b8cff4d8f 76 }
tvlogman 15:b76b8cff4d8f 77
vsluiter 0:c8f15874531b 78 }
tvlogman 7:1bffab95fc5f 79