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

Committer:
tvlogman
Date:
2017-09-29
Revision:
18:65e24db15c69
Parent:
17:616ce7bc1f96

File content as of revision 18:65e24db15c69:

#include "mbed.h"
#include "MODSERIAL.h"
#include "HIDScope.h"
#include "QEI.h"
#include "FastPWM.h"

enum robotStates {KILLED, ACTIVE};
robotStates currentState = KILLED;

QEI Encoder(D12,D13,NC,64, QEI::X4_ENCODING);
MODSERIAL pc(USBTX, USBRX);

// Defining outputs
DigitalOut motor1_direction(D4);
PwmOut motor1_pwm(D5);

// Defining inputs
InterruptIn sw2(SW2);
InterruptIn sw3(SW3);
InterruptIn button1(D2);
AnalogIn pot(A0);

// PWM settings
float pwmPeriod = 1.0/5000.0;
int frequency_pwm = 10000; //10kHz PWM

volatile float x;
volatile float x_prev =0; 
volatile float y; // filtered 'output' of ReadAnalogInAndFilter 

// Initializing encoder
Ticker encoderTicker;
volatile int counts = 0;
volatile float revs = 0.00;

void readEncoder(){
    counts = Encoder.getPulses();
    revs = counts/(131*64.0f);
    pc.printf("%0.2f revolutions \r\n", revs);
    
    // Displaying revs in HIDscope
    x = revs;   // Capture data
    y = (x_prev + x)/2.0;   // averaging filter
    x_prev = x; // Prepare for next round
    
    }
    
void killSwitch(){
    pc.printf("Motors turned off");
    currentState = KILLED;
    }
    
void turnMotorsOn(){
    pc.printf("Motors turned on");
    currentState = ACTIVE; 
    }

    
void M1switchDirection(){
    motor1_direction = !motor1_direction;
    }

int main()
    {
    pc.baud(115200);
    pc.printf("Entered main function");
    motor1_direction = false;
    motor1_pwm.period(pwmPeriod);//T=1/f 
    sw2.fall(&killSwitch);
    sw3.fall(&turnMotorsOn);
    button1.rise(&M1switchDirection);
    
    encoderTicker.attach(readEncoder, 0.1);
      
    pc.printf("Encoder ticker attached and baudrate set");
    
    while(true){
            switch(currentState){
                case KILLED:
                    motor1_pwm.write(0.0);
                    break;
                case ACTIVE:
                    motor1_pwm.write(pot.read()); // Motor speed proportional to pot meter                
                    break;
            }  
        }
    
    }