biorobotics group 19 , 2 november
Dependencies: HIDScope MODSERIAL PID QEI SigInterpreternovember2 biquadFilter mbed
Fork of Robot_controlv2 by
Diff: Controller.h
- Revision:
- 6:a4440eec3652
- Parent:
- 4:bfdcf3da7cff
--- a/Controller.h Fri Oct 28 12:50:44 2016 +0000 +++ b/Controller.h Wed Nov 02 15:39:10 2016 +0000 @@ -2,27 +2,28 @@ SigInterpreter signal = SigInterpreter(); const double PI = 3.141592653589793; -//MODSERIAL pc(USBTX, USBRX); + +MODSERIAL pc(USBTX, USBRX); +//HIDScope scope(2); int xPulses = 0; // pulsecount -const int x_max = 33068; +const int x_max = 30068;// 30068; int yPulses = 0; -const int y_max = 17640; +const int y_max = 15003; 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 maxXValue = 1.0; // max motorvalue +const float maxYValue = 1.0; -const float maxSpeed = 0.01f; // m/s +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 @@ -31,51 +32,53 @@ float prevX = 0; float prevY = 0; -double r = 0; -double l = 0; -double u = 0; -double d = 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 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 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.001; // rate = interval motors will be updateed +const float RATE = 0.002; // rate = interval motors will be updateed -int counter = 0; -int counter_motor = 0; +int counter = 1; 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); +PID X_Controller(KcX, Ti, Td, RATE); +QEI X_Motor(D13 ,D12 ,NC, 64 ,QEI::X4_ENCODING); -PwmOut Y_Magnitude(D6); // bind to Ytical motor +PwmOut Y_Magnitude(D6); // bind to vertical motor DigitalOut Y_Direction(D7); -PID Y_Controller(Kc, Ti, Td, RATE); -QEI Y_Motor(D14 ,D15 ,NC, 64 ,QEI::X4_ENCODING); +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 = true; +const int CW = false; DigitalOut led(LED_BLUE); @@ -87,15 +90,16 @@ Y_Magnitude.period_ms(1); Y_Magnitude.write(0.0); Y_Direction.write(CW); + led = true; } void initControllers(){ - X_Controller.setInputLimits(-1.0,1.0); // fiX_ me + X_Controller.setInputLimits(-maxSpeed,maxSpeed); // max output will be 10% X_Controller.setOutputLimits(-1.0,1.0); X_Controller.setBias(0); - X_Controller.setMode(0); // fix me + X_Controller.setMode(1); - Y_Controller.setInputLimits(-1.0,1.0); // fix me + Y_Controller.setInputLimits(-maxSpeed,maxSpeed); // fix me Y_Controller.setOutputLimits(-1.0,1.0); Y_Controller.setBias(0); - Y_Controller.setMode(0); + Y_Controller.setMode(1); }