#include "Gyro.h"

#include "WPILib.h"

#include "Sensors.h"

#include "rebootLib.h"

#include "CANJaguar.h"

#include "math.h"

#include "DashboardDataFormat.h"

//.06 addition to all but outside top

//PORT 6

//these constants change where the arm can move in certain positions

const float OFFSET=.094;//((10.0f/300.0f)+.06);

const int MAX_POS_0=210;

const int MAX_POS_1=300;

const int MAX_POS_2=300;

const int MAX_POS_3=300;

const float FOLD=(7.0f/300.0f);

const float MIDDLE_LOW_ANGLE=(.406)-OFFSET;//(120.0f/300.0f);

const float MIDDLE_TOP_ANGLE=((.684)-OFFSET);

const float TOP_LOW_ANGLE=.836-OFFSET;//(240.0f/300.0f);

const float TOP_TOP_ANGLE=.760-OFFSET;//(247.0f/300.0f);

const float GROUND=(.6219);

const float POT_GAIN[6]={(60.0f/300.0f),(60.0f/300.0f),(60.0f/300.0f),(60.0f/300.0f),(20.0f/300.0f),0};

const bool LEFT=true;//constants for possible auton change;

const bool RIGHT=false;

const float OUTSIDE=.22;

const float INSIDE=.15;

const float CENTERY=60;

const float FORWARDY=139;

const float STRAIGHT=180;//was 170

const int CAMERA_FLOOR=55;

const int CAMERA_CENTER=85;

const float ARM_P=-1500;

int position;

int mode; //Autonomous, leave it for now.

bool compressorState; //True==On

bool armState;

#define _NEWCYP // Must be defined

#define _CYPWRAP // Define to use Joystick #2 for inputs

// Don't define to use Cypress Box for inputs

#ifndef _NEWCYP

class Cypress { // This code wont compile under any configuration of _NEWCYP or _CYPWRAP

}

float Cypress::GetAnalogIn(int channel) {

}

bool Cypress::GetClaw()

{

return GetDigitalIn(3);

}

};

#else

#ifndef _CYPWRAP

class Cypress { //

};

#else

class Cypress{ // Using Joystick unber 2 for "Cypress" inputs

};

#endif

#endif

class RawControl {

} else

low->Set(DoubleSolenoid::kReverse);

if (two) {

high->Set(DoubleSolenoid::kForward);

} else

high->Set(DoubleSolenoid::kReverse);

}

//CHECK THIS OUT!!

void RawControl::configJags() {

}

void RawControl::claw(bool open) {

gripper->Set(m_Cypress->GetClaw());

}

int RawControl::zero() {

}

/* gets stick values into the x y and z values, these values are not returned by this but rather stored into the respective variables, the values are filtered*/

void RawControl::getStickValues(float &x, float &y, float &z) {

}

float RawControl::averageEncoders()

}

void RawControl::resetEncoders()

}

float RawControl::getArmPos()

{

return arm->GetPosition();

}

};

class FunctionBot {