Group19-Biorobotics
Dependencies: HIDScope MODSERIAL PID QEI SigInterpreternovember2 biquadFilter mbed
Fork of Robot_control_v3 by
Controller.h
- Committer:
- fabian101
- Date:
- 2016-10-28
- Revision:
- 4:bfdcf3da7cff
- Parent:
- 2:6b913f93bc0b
- Child:
- 6:a4440eec3652
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); }