[TikiTech]

#include "WPILib.h"
#include "Math.h"
//#include "VisionProcess.cpp"

int shot = 5;
Victor *intakeL;
Victor *intakeR;
Gyro *gyro;
Encoder *encoderL;
Encoder *encoderR;
ADXL345_I2C *accel;
//DriverStationLCD *driverStation;
DigitalInput *pressure;
Relay *presser;
Servo *cameraTilt;
Servo *cameraTurn;
AnalogChannel *armPotent;
class RobotDemo : public SimpleRobot
{
Talon yLeft;
Talon sLeft;
Talon yRight;
Talon sRight;
Joystick stick; // only joystick
Joystick assistick;
Jaguar armL;
Jaguar armR;
Solenoid shooterL;
Solenoid shooterR;
Relay shifter1;
Relay shifter2;
static const float Kp = 0.05;
int ang;

public:
RobotDemo():
yLeft(1),
sLeft(2),
yRight(3),
sRight(4),
stick(1), // as they are declared above.really are they ? not sure maybe.
assistick(2),
armL(5),
armR(6),
shooterL(1),
shooterR(2),
shifter1(2),
shifter2(3)
{
GetWatchdog().SetEnabled(false);
yLeft.SetExpiration(0.1);
sLeft.SetExpiration(0.1);
yRight.SetExpiration(0.1);
sRight.SetExpiration(0.1);
armL.SetExpiration(0.1);
armR.SetExpiration(0.1);
intakeL = new Victor(7);
intakeR = new Victor(10);
gyro = new Gyro(1);
encoderL = new Encoder(6,7);
encoderR = new Encoder(8,9);
pressure = new DigitalInput(5);
presser = new Relay(1);
cameraTilt = new Servo(9);
cameraTurn = new Servo(8);
armPotent = new AnalogChannel(2);
//driverStation = DriverStationLCD->GetInstance();
//ADXL345_I2C- = new Accelerometer(1);
}

/**
* Drive left & right motors for 2 seconds then stop
*/

void RobotInit()
{
//SmartDashboard::init();
encoderL->Start();
encoderR->Start();
}
void Autonomous()//range: 6ft-
{
//cout<<"in auton \n";
encoderL->Reset();
encoderR->Reset();
Wait(0.5);
int t = 0;
while((fabs(encoderL->GetRaw())<4500||fabs(encoderR->GetRaw())<4500) && t<200)
{
yLeft.Set(0.7);
sLeft.Set(-0.7);
yRight.Set(-0.7);
sRight.Set(0.7);
Wait(0.005);
t++;
}
t = 0;
while((fabs(encoderL->GetRaw())<4700||fabs(encoderR->GetRaw())<4700) && t<200)
{
yLeft.Set(0.4);
sLeft.Set(-0.4);
yRight.Set(-0.4);
sRight.Set(0.4);
Wait(0.005);
}
yLeft.Set(0.0);
sLeft.Set(0.0);
yRight.Set(0.0);
sRight.Set(0.0);
armL.Set(0.5);
armR.Set(0.5);
Wait(0.8);
armL.Set(0.0);
armR.Set(0.0);
Wait(0.2);
shooterL.Set(true);
shooterR.Set(true);
Wait(0.3);
shooterL.Set(false);
shooterR.Set(false);
Wait(0.2);
t = 0;
while((fabs(encoderL->GetRaw())>0||fabs(encoderR->GetRaw())>0) && t<150)
{
yLeft.Set(-0.7);
sLeft.Set(0.7);
yRight.Set(0.7);
sRight.Set(-0.7);
intakeL->Set(0.6);
intakeR->Set(-0.6);
Wait(0.005);
t++;
}
t = 0;
while((fabs(encoderL->GetRaw())<250||fabs(encoderR->GetRaw())<250) && t<150)
{
yLeft.Set(-0.5);
sLeft.Set(0.5);
yRight.Set(0.5);
sRight.Set(-0.5);
Wait(0.005);
t++;
}
yLeft.Set(0.0);
sLeft.Set(0.0);
yRight.Set(0.0);
sRight.Set(0.0);
Wait(1.5);
intakeL->Set(0.0);
intakeR->Set(0.0);
Wait(0.5);
t = 0;

while((fabs(encoderL->GetRaw())<2000||fabs(encoderR->GetRaw())<2000) && t<150)
{
yLeft.Set(0.7);
sLeft.Set(-0.7);
yRight.Set(-0.7);
sRight.Set(0.7);
Wait(0.005);
t++;
}

t = 0;
while((fabs(encoderL->GetRaw())<2100||fabs(encoderR->GetRaw())<2100) && t<150)
{
yLeft.Set(0.4);
sLeft.Set(-0.4);
yRight.Set(-0.4);
sRight.Set(0.4);
Wait(0.005);
t++;
}
yLeft.Set(0.0);
sLeft.Set(0.0);
yRight.Set(0.0);
sRight.Set(0.0);
Wait(0.3);
shooterL.Set(true);
shooterR.Set(true);
Wait(0.3);
shooterL.Set(false);
shooterR.Set(false);
//cout<< "out of auton \n";
}

/**
* Runs the motors with arcade steering.
*/
//////////DRIVESTICK CONFIG////////// ********** = TO DO //////////ASSISTICK CONFIG//////////
// Btn 1 A Button //null // Btn 1 1 Button //
// Btn 2 B Button //SHOOTER Safety Button (Both buttons // Btn 2 2 Button //
// Btn 3 X Button //null must be pressed // Btn 3 3 Button //
// Btn 4 Y Button //SHOOTER Fire Button to shoot) // Btn 4 4 Button //
// Btn 5 L Bumper //TOOL OUT/PASS ********** //
// Btn 6 R Bumper //TOOL INTAKE **********
// Btn 7 Back Btn //null
// Btn 8 Start Btn //null
// Btn 9 L Joy Click//null
// Btn 10 R Joy Click//null
// Axis 1 L X Axis //null
// Axis 2 L Y Axis //DRIVE +Forward/Reverse-
// Axis 3 + L Trigger //ARM DOWN **********
// Axis 3 - R Trigger //ARM UP **********
// Axis 4 R X Axis //STEER +Right/Left-
// Axis 5 R Y Axis //null


void OperatorControl()
{
//cout<<"In Operator Contrl";
//int logcounter = 200;
//int *CamTurn;
//int *CamTilt;
//bool *Pv;
int h=1;
//float *P;
int *angle;
while (IsOperatorControl())
{
/*if(logcounter >0)
{
cout<<"running userctrl \n";
logcounter--;
}*/
*angle = int(gyro->GetAngle());

if(((stick.GetRawAxis(2))>0.15 || stick.GetRawAxis(2)<(-0.15)) || ((stick.GetRawAxis(4)>0.10) || (stick.GetRawAxis(4)<-0.10)))
{
/*if(logcounter >0)
{
cout<<"running drive \n";
logcounter--;
}*/
yLeft.Set(-stick.GetRawAxis(2)+(stick.GetRawAxis(4)));
yRight.Set(stick.GetRawAxis(2)+(stick.GetRawAxis(4 )));
sLeft.Set(stick.GetRawAxis(2)-(stick.GetRawAxis(4)));
sRight.Set(-stick.GetRawAxis(2)-(stick.GetRawAxis(4)));
}
else if((fabs(stick.GetRawAxis(2))>0.15) && ((fabs(stick.GetRawAxis(4))<0.10)))
{
/*if(logcounter >0)
{
cout<<"running straightdrive \n";
logcounter--;
}*/
yLeft.Set(-stick.GetRawAxis(2)-(*angle*Kp));
yRight.Set(stick.GetRawAxis(2)+(*angle*Kp));
sLeft.Set(stick.GetRawAxis(2)+(*angle*Kp));
sRight.Set(-stick.GetRawAxis(2)-(*angle*Kp));
}
else
{
yLeft.Set(0);
yRight.Set(0);
sLeft.Set(0);
sRight.Set(0);
}
if(assistick.GetRawButton(5)) //ARM UP
{
//cout<<"arm up \n";
armR.Set(-0.5);
armL.Set(-0.5);
}
else if(assistick.GetRawButton(6)) //ARM DOWN
{
//cout<<"arm down \n";
armR.Set(0.5);
armL.Set(0.5);
}
else if(fabs(stick.GetRawAxis(3)) > 0.3 && !assistick.GetRawButton(5) && !assistick.GetRawButton(6)){
armR.Set(stick.GetRawAxis(3));
armL.Set(stick.GetRawAxis(3));
}
else{
armR.Set(0.0);
armL.Set(0.0);
}

if(!pressure->Get())
{
presser->Set(presser->kOn);
}
else if(stick.GetRawButton(1)||pressure->Get())
{
presser->Set(presser->kOff);
}

if(stick.GetRawButton(10))
{
shifter1.Set(shifter1.kOn);
shifter2.Set(shifter2.kOff);
Wait(0.2);
}
else if(stick.GetRawButton(9))
{
shifter1.Set(shifter1.kOff);
shifter2.Set(shifter2.kOn);
Wait(0.2);
}
else
{
shifter1.Set(shifter1.kOff);
shifter2.Set(shifter2.kOff);
}

if(stick.GetRawButton(4) && stick.GetRawButton(3) && !assistick.GetRawButton(2))// && stick.GetAxis(3)>0)
{
shooterL.Set(true);
shooterR.Set(true);
Wait(0.25);
}
else if(stick.GetRawButton(4) && stick.GetRawButton(3) && assistick.GetRawButton(2))// && stick.GetAxis(3)>0)
{
shooterL.Set(true);
shooterR.Set(true);
Wait(0.10);
}

else
{
shooterL.Set(false);
shooterR.Set(false);
}

if(stick.GetRawButton(6) || assistick.GetRawButton(7)) //TOOL INTAKE
{
//cout<<"intake on\n";
intakeL->Set(0.8);
intakeR->Set(-0.8);
}

else if((stick.GetRawButton(5) || assistick.GetRawButton(8)) && !assistick.GetRawButton(7)) //TOOL OUT/PASS
{
//cout<< "pass on \n";
intakeL->Set(-1.0);
intakeR->Set(1.0);
}
else
{
intakeL->Set(0);
intakeR->Set(0);
}
if (h%48 == 0)
{
//*P = armPotent->GetAverageValue();
//*Pv = presser->Get();
//*CamTilt = int(cameraTilt->Get());
//*CamTurn = int(cameraTurn->Get());
//*Pot = int(armPotent->GetValue()); *G = int(gyro->GetAngle());
//cout<<("LeftEncoder: "&& *L &&" RightEncoder: "&& *R &&" Gyro:"&& *G &&" Pressure Switch: "&& *Ps &&" Compressor: "&& *Pv);
///*cout<<("ArmPotentiometer: "); cout<<(*Pot);cout<<("\ncamTurn: ");cout<<(*CamTurn);cout<<("\nGyroscope Angle: ");cout<<(*G);cout<<"\n";
//cout<<"ArmPotent: "<<*P<<"\n";
}
h++;
Wait(0.03); // wait for a motor update time
}
//cout<< "out of usercntrl \n";
}

void Test() {

}
};

START_ROBOT_CLASS(RobotDemo);