Controller.h

00001 const double PI = 3.141592653589793;
00002 
00003 int xPulses = 0; // pulsecount
00004 int x_max = 33068;
00005 int yPulses = 0;
00006 int y_max = 17640;
00007 int prevXpulses = 0;
00008 int prevYpulses = 0;
00009 
00010 double rMax; // maxAmplitude emg signal
00011 double lMax;
00012 double uMax;
00013 double dMax;
00014 
00015 float motorValueX = 0; // requested speed
00016 float motorValueY = 0;
00017 
00018 const float maxXValue = 1.0f; // v1 in drawing, max speed cm/s
00019 const float maxYValue = 1.0f; // 
00020 
00021 const float maxSpeed = 0.01f; // m/s
00022 
00023 const float R1 = 0.085; // radius of big driven gear in m
00024 const float RmX = 0.012; // radius of small drive gear in m   
00025 const float Rpulley = 0.015915; // radius of pulley gear in m
00026 
00027 float prevX = 0;
00028 float prevY = 0;
00029 
00030 double r = 0;
00031 double l = 0;
00032 double u = 0;
00033 double d = 0;
00034 int i = 1;
00035 
00036 int SAMPLES_PER_AVERAGE = 10;
00037 const int margin = 100; // margin pulses for calibration
00038 
00039 void calibrate();
00040 void calibratePower();
00041 void run();
00042 double toMotorValue(double,double);
00043 void emergencyExit();
00044 void motorOutput(double, bool);
00045 void initControllers();
00046 void initMotors();
00047 void initMotorvariables();
00048 
00049 SigInterpreter signal = SigInterpreter();
00050 PulseChecker pulses =  PulseChecker();
00051 
00052 const float Kc = 0.0;
00053 const float Ti = 0.0;
00054 const float Td = 0.0;
00055 float RATE = 0.01; // rate = interval motors will be updateed
00056 
00057 PwmOut X_Magnitude(D5); // bind to horizontal motor
00058 DigitalOut X_Direction(D4);
00059 PID X_Controller(Kc, Ti, Td, RATE);
00060 
00061 PwmOut Y_Magnitude(D6); // bind to Ytical motor
00062 DigitalOut Y_Direction(D7);
00063 PID Y_Controller(Kc, Ti, Td, RATE);
00064 
00065 InterruptIn calibrateButton(D2);
00066 Ticker motorControl;
00067 void setFlag();
00068 bool flag = true;
00069 bool calibrateFlag = true;
00070 
00071 int CW = true;
00072 
00073 void initMotors(){
00074     X_Magnitude.period_us(50);
00075     X_Magnitude.write(0.0);
00076     X_Direction.write(CW);
00077     
00078     Y_Magnitude.period_us(50);
00079     Y_Magnitude.write(0.0);
00080     Y_Direction.write(CW);
00081 }
00082 void initControllers(){
00083     X_Controller.setInputLimits(-90.0,90.0); // fiX_ me
00084     X_Controller.setOutputLimits(-1.0,1.0);
00085     X_Controller.setBias(0);
00086     X_Controller.setMode(0);
00087     
00088     Y_Controller.setInputLimits(-90.0,90.0); // fix me
00089     Y_Controller.setOutputLimits(-1.0,1.0);
00090     Y_Controller.setBias(0);
00091     Y_Controller.setMode(0);
00092 }