File content as of revision 7:af586102d80c:

#ifndef CONSTANTS
#define CONSTANTS

PwmOut pwmpin1(D6);
PwmOut pwmpin2(D5);
AnalogIn potmeter1(A5);
AnalogIn potmeter2(A4);
DigitalIn button1(D2);
DigitalIn button2(D3);
DigitalOut directionpin2(D4);
DigitalOut directionpin1(D7);
QEI motor1(D13,D12,NC, 32);
QEI motor2(D11,D10,NC, 32);

//Define objects
AnalogIn    emg0( A0 );     // EMG at A0
BiQuad emg0bq1(0.8848578, -1.7697156, 0.8848578, -1.7539023, 0.7855289); // highpass at 30Hz  Q at around 1
BiQuad emg0bq2(0.0773021,0.1546042,0.0773021,-1.3098283,0.6190368); // lowpass at 130 Hz Q at around .6
BiQuad emg0bq3(0.9556457,-1.81774618, 0.955645, -1.817746, 0.9112914); // 50 Hz notch Q at 4.5
BiQuadChain emg0bqc; // merged chain of three filters

AnalogIn    emg1( A1 );     // EMG at A1
BiQuad emg1bq1(0.8848578, -1.7697156, 0.8848578, -1.7539023, 0.7855289); // highpass at 30Hz  Q at around 1
BiQuad emg1bq2(0.0773021,0.1546042,0.0773021,-1.3098283,0.6190368); // lowpass at 130 Hz Q at around .6
BiQuad emg1bq3(0.9556457,-1.81774618, 0.955645, -1.817746, 0.9112914); // 50 Hz notch Q at 4.5
BiQuadChain emg1bqc; // merged chain of three filters

AnalogIn    emg2( A2 );     // EMG at A2
BiQuad emg2bq1(0.8848578, -1.7697156, 0.8848578, -1.7539023, 0.7855289); // highpass at 30Hz  Q at around 1
BiQuad emg2bq2(0.0773021,0.1546042,0.0773021,-1.3098283,0.6190368); // lowpass at 130 Hz Q at around .6
BiQuad emg2bq3(0.9556457,-1.81774618, 0.955645, -1.817746, 0.9112914); // 50 Hz notch Q at 4.5
BiQuadChain emg2bqc; // merged chain of three filters

InterruptIn kill_switch(SW2); //position has to be changed
DigitalIn next_switch(SW3); //name and position should be replaced

enum position_states{PositionCalibration, EmgCalibration, Movement, KILL};
enum emg_states{EMG0, EMG1, EMG2};
position_states CurrentState;
emg_states CalibrationState;

Ticker sample_timer;
Ticker MotorsTicker;
Ticker stateticker;
Timer timer;

//for testing purposes
DigitalOut ledred(LED_RED);
DigitalOut ledgreen(LED_GREEN);
DigitalOut ledblue(LED_BLUE);
//MODSERIAL pc(USBTX, USBRX);
//HIDScope scope(6);

bool    emg0Bool    = 0;        // I don't know if these NEED to be global, but when I tried to put them in they wouldn't work...
int     emg0Ignore  = 0;
bool    emg1Bool    = 0;
int     emg1Ignore  = 0;
bool    emg2Bool    = 0;
int     emg2Ignore  = 0;
float   Calibration0;
float   Calibration1;
float   Calibration2;

double  input = 0;              // raw input
double  filtHigh = 0;           // filtered after highpass
double  filtlow = 0;            // filtered after lowpass
double  filtNotch = 0;          // filtered after notch
double  emg0filteredAbs;

float threshold0;
float threshold1;
float threshold2;

volatile float pwm_value1 = 0.0;
volatile float pwm_value2 = 0.0;

const float C1 = 3.0; //motor 1 gear ratio
const float C2 = 0.013; //motor 2 gear ratio/radius of the circular gear in m
const float length = 0.300; //length in m (placeholder)
const float Timestep = 0.01;

volatile bool x_direction = true;

volatile float x_position = length;
volatile float y_position = 0.0;
volatile float old_x_position;
volatile float old_y_position;
volatile float x_correction;
volatile float old_x_correction;
volatile float old_motor1_angle;
volatile float old_motor2_angle;
volatile float motor1_angle = 0.0; //sawtooth gear motor
volatile float motor2_angle = 0.0; //rotational gear motor
volatile float direction;

//values of PID controller
const float Kp = 5;
const float Ki = 0.02;
const float Kd = 0.05;
volatile float Output1 = 0 ;      //Starting value
volatile float Output2 = 0 ;      //Starting value
volatile float P1 = 0;           //encoder value
volatile float P2 = 0;
volatile float e1 = 0 ;          //Starting value 
volatile float e2 = 0 ;          //Starting value
volatile float e3 = 0;  
volatile float f1 = 0 ;          //Starting value 
volatile float f2 = 0 ;          //Starting value
volatile float f3 = 0; 
volatile float df3 = 0;
volatile float if3 = 0;
volatile float de3 = 0;
volatile float ie3 = 0;

volatile float Output_Last1;      // Remember previous position
volatile float Output_Last2;      // Remember previous position
volatile float Y1 = 0;               // Value that is outputted to motor control
volatile float Y2 = 0;               // Value that is outputted to motor control
volatile float P_Last = 0;       // Starting position

#endif