File content as of revision 1:b9005f2aabf5:

InterruptIn calibrateButton(D2);

const double PI = 3.141592653589793;

int x = 0; // pulsecount
int x_max; // ??? physically measure
int y = 0;
int y_max; // ???

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

float minHorThreshold; // threshold emgsignal
float minVerThreshold;

float motorValueHor = 0; // requested speed
float motorValueVer = 0;

const float maxHorValue = 1.0f; // v1 in drawing, max speed cm/s
const float maxVerValue = 1.0f; // 

const float maxSpeed = 1.0f; // cm/s

const float R1 = 8.5; // radius of big driven gear in Cm
const float RmHor = 0.5; // radius of small drive gear in Cm   
const float RmVer = 1.5915; // radius of pulley gear in Cm

const float maxRotSpeedHor = maxSpeed/RmHor; // w_m = v1/r2 rad/s 
const float maxRotSpeedVer = maxSpeed/RmVer; // w_m = v1/r1 rad/s

float currentX = 0; // CM
float currentY = 0;

float toX = 0; // CM
float toY = 0;

float currentXangle = 0; // degrees
float currentYangle = 0;

float toangleX = 0; // degrees
float toangleY = 0; 

float Xvelrequest = 0; // degrees/sec
float Yvelrequest = 0;

float prevX = 0; // pulses
float prevY = 0;

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();

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

const float Kc = 0.0;
const float Ti = 0.0;
const float Td = 0.0;
float RATE = 1/1000; // ???

PwmOut horizontalMagnitude(D5); // bind to horizontal motor
DigitalOut horizontalDirection(D4);
PID horizontalController(Kc, Ti, Td, RATE);

PwmOut verticalMagnitude(D6); // bind to vertical motor
DigitalOut verticalDirection(D7);
PID verticalController(Kc, Ti, Td, RATE);

Ticker motorControl;
void setFlag();
bool flag = true;

void initMotors(){
    horizontalMagnitude.period_us(50);
    horizontalMagnitude.write(0.0);
    horizontalDirection.write(1);
    
    verticalMagnitude.period_us(50);
    verticalMagnitude.write(0.0);
    verticalDirection.write(1);
}
void initControllers(){
    horizontalController.setInputLimits(-90.0,90.0); // fix me
    horizontalController.setOutputLimits(-1.0,1.0);
    horizontalController.setBias(0);
    horizontalController.setMode(0);
    
    verticalController.setInputLimits(-90.0,90.0); // fix me
    verticalController.setOutputLimits(-1.0,1.0);
    verticalController.setBias(0);
    verticalController.setMode(0);
}