File content as of revision 2:6b913f93bc0b:

const double PI = 3.141592653589793;

int xPulses = 0; // pulsecount
int x_max = 33068;
int yPulses = 0;
int y_max = 17640;
int prevXpulses = 0;
int prevYpulses = 0;

double rMax; // maxAmplitude emg signal
double lMax;
double uMax;
double dMax;

float motorValueX = 0; // requested speed
float motorValueY = 0;

const float maxXValue = 1.0f; // v1 in drawing, max speed cm/s
const float maxYValue = 1.0f; // 

const float maxSpeed = 0.01f; // m/s

const float R1 = 0.085; // radius of big driven gear in m
const float RmX = 0.012; // radius of small drive gear in m   
const float Rpulley = 0.015915; // radius of pulley gear in m

float prevX = 0;
float prevY = 0;

double r = 0;
double l = 0;
double u = 0;
double d = 0;
int i = 1;

int SAMPLES_PER_AVERAGE = 10;
const int margin = 100; // margin pulses for calibration

void calibrate();
void calibratePower();
void run();
double toMotorValue(double,double);
void emergencyExit();
void motorOutput(double, bool);
void initControllers();
void initMotors();
void initMotorvariables();

SigInterpreter signal = SigInterpreter();
PulseChecker pulses =  PulseChecker();

const float Kc = 0.0;
const float Ti = 0.0;
const float Td = 0.0;
float RATE = 0.01; // rate = interval motors will be updateed

PwmOut X_Magnitude(D5); // bind to horizontal motor
DigitalOut X_Direction(D4);
PID X_Controller(Kc, Ti, Td, RATE);

PwmOut Y_Magnitude(D6); // bind to Ytical motor
DigitalOut Y_Direction(D7);
PID Y_Controller(Kc, Ti, Td, RATE);

InterruptIn calibrateButton(D2);
Ticker motorControl;
void setFlag();
bool flag = true;
bool calibrateFlag = true;

int CW = true;

void initMotors(){
    X_Magnitude.period_us(50);
    X_Magnitude.write(0.0);
    X_Direction.write(CW);
    
    Y_Magnitude.period_us(50);
    Y_Magnitude.write(0.0);
    Y_Direction.write(CW);
}
void initControllers(){
    X_Controller.setInputLimits(-90.0,90.0); // fiX_ me
    X_Controller.setOutputLimits(-1.0,1.0);
    X_Controller.setBias(0);
    X_Controller.setMode(0);
    
    Y_Controller.setInputLimits(-90.0,90.0); // fix me
    Y_Controller.setOutputLimits(-1.0,1.0);
    Y_Controller.setBias(0);
    Y_Controller.setMode(0);
}