File content as of revision 8:82e38f4aa690:

#include "SigInterpreter.h"

SigInterpreter signal = SigInterpreter();
const double PI = 3.141592653589793;

MODSERIAL pc(USBTX, USBRX);
//HIDScope scope(2);

int xPulses = 0; // pulsecount
const int x_max = 31068;// 30068;
int yPulses = 0;
const int y_max = 15003;
int prevXpulses = 0;
int prevYpulses = 0;

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

const float maxXValue = 1.0; // max motorvalue
const float maxYValue = 1.0; 

const float maxSpeed = 1; // cm/s

float X_frictionTrheshold = 0.04;
float Y_frictionTrheshold = 0.04; 
float sensitivityFactor = 0.2; // determines treshold value for contraction, value between 0 and 1
float calibrationSpeed = 0.4; // [0,1], determines how fast the system returns to (0,0) when the button is pressed

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;

float r = 0;
float l = 0;
float u = 0;
float d = 0;
float rMax = 0; // maxAmplitude emg signal
float lMax = 0;
float uMax = 0;
float dMax = 0;
float percentageR = 0;
float percentageL = 0;
float percentageU = 0;
float percentageD = 0;

int i = 0;

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

void calibrate();
void calibratePower();
void run();
void initControllers();
void initMotors();

const float KcX = 0.02058; // THIS IS THE LIMIT FOR STABILITY
const float KcY = 0.02066;
const float Ti = 0.0;
const float Td = 0.0;
const float RATE = 0.002; // rate = interval motors will be updateed

int counter = 1;

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

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

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

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);
    led = true;
}
void initControllers(){
    X_Controller.setInputLimits(-maxSpeed,maxSpeed); // max output will be 10%
    X_Controller.setOutputLimits(-1.0,1.0);
    X_Controller.setBias(0);
    X_Controller.setMode(1); 
    
    Y_Controller.setInputLimits(-maxSpeed,maxSpeed); // fix me
    Y_Controller.setOutputLimits(-1.0,1.0);
    Y_Controller.setBias(0);
    Y_Controller.setMode(1);
}