File content as of revision 4:bfdcf3da7cff:

#include "SigInterpreter.h"

SigInterpreter signal = SigInterpreter();
const double PI = 3.141592653589793;
//MODSERIAL pc(USBTX, USBRX);

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

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

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 = 0;

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

HIDScope scope(2);

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

int counter = 0;
int counter_motor = 0;

PwmOut X_Magnitude(D5); // bind to horizontal motor
DigitalOut X_Direction(D4);
PID X_Controller(Kc, Ti, Td, RATE);
QEI X_Motor(D12 ,D13 ,NC, 64 ,QEI::X4_ENCODING); 

PwmOut Y_Magnitude(D6); // bind to Ytical motor
DigitalOut Y_Direction(D7);
PID Y_Controller(Kc, Ti, Td, RATE);
QEI Y_Motor(D14 ,D15 ,NC, 64 ,QEI::X4_ENCODING); 

//InterruptIn calibrateButton(D2);
DigitalIn button(D2);
Ticker motorControl;
void setUpdate_flag();
volatile bool update_flag = false;
const int CW = true;

DigitalOut led(LED_BLUE);

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