biorobotics group 19 , 2 november
Dependencies: HIDScope MODSERIAL PID QEI SigInterpreternovember2 biquadFilter mbed
Fork of Robot_controlv2 by
Controller.h
- Committer:
- fabian101
- Date:
- 2016-11-02
- Revision:
- 7:2ab4bdd1f998
- Parent:
- 6:a4440eec3652
File content as of revision 7:2ab4bdd1f998:
#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 = 30068;// 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 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.0358; // THIS IS THE LIMIT FOR STABILITY const float KcY = 0.366; 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); }