/*
THIS CODE IS MADE BY THE BEST PEOPLE IN THE WORLD, DO NOT STEAL.
THINGS TO ARE IN CAPS
Instructions are in sentence form.
*/
package edu.wpi.first.wpilibj.templates;
import edu.wpi.first.wpilibj.*;
public class Robot4334 extends SimpleRobot {
//DECLARE ALL METHODS IN INIT AND SIMPLEROBOT
public RobotDrive drivetrain;
public Joystick XBoxController;
public Gyro gyro;
public Timer timer;
public Encoder encoder;
public static final double DRIVE_STRAIGHT = 0;
public static final double DRIVE_FULL_SPEED = 1;
public static final int LeftX = 1;
public static final int LeftY = 2;
public static final int Triggers = 3; //(Each trigger = 0 to 1, axis value = right - left)
public static final int RightX = 4;
public static final int RightY = 5;
public static final int DPad = 6;
public static void log(String theMessage)
{ // Logger -System.out.println function
System.out.println(theMessage);
}
public void BRAKE()
{ //Brake method - stops motors
log("Brake on");
drivetrain.drive(0.0,0.0);
drivetrain.stopMotor();
}
public void EMERGENCY()
{ //ADD ALL FUNCTIONS HERE TO STOP ON ROBOT DURING EMERGENCY
log("Emergency");
drivetrain.drive(0.0,0.0);
Timer.delay(10.0);
}
public void RESET()
{
gyro.reset();
timer.reset();
drivetrain.stopMotor();
}
public void stopAndResetTimer()
{
timer.stop();
timer.reset();
}
public boolean getBrake()
{
boolean brakeOn = false;
if(XBoxController.getRawButton(2))
{
BRAKE();
brakeOn = true;
}
return brakeOn;
}
public void getEmergency()
{
if(XBoxController.getRawButton(5))
{
EMERGENCY();
}
}
public double getTimer()
{
double time = timer.get();
return time;
}
public void startTimer()
{
timer.start();
}
public double restartTimer()
{
double time = timer.get();
timer.stop();
timer.reset();
timer.start();
return time;
}
public double returnSpeed()
{
double speed = XBoxController.getRawAxis(Triggers);
//PERFORMANCE CURVE LOGIC GOES HERE
return speed;
}
public float returnTurn(char LorR)
{
float Turn;
if(XBoxController.getRawAxis(Triggers)>0.1 || XBoxController.getRawAxis(Triggers)<-0.1)
{
Turn = (float) returnSpeed();
}
else if(LorR == 'L' && XBoxController.getRawAxis(Triggers)<0.1 && XBoxController.getRawAxis(Triggers)>-0.1)
{
if(XBoxController.getRawAxis(LeftY)<= 0.05 && XBoxController.getRawAxis(LeftY)>= -0.05)
Turn = 0;
else
Turn = (float) XBoxController.getRawAxis(LeftY);
}
else if(LorR == 'R' && XBoxController.getRawAxis(Triggers)<0.1 && XBoxController.getRawAxis(Triggers)>-0.1)
{
if(XBoxController.getRawAxis(RightY)<= 0.05 && XBoxController.getRawAxis(RightY)>= -0.05)
Turn = 0;
else
Turn = (float) XBoxController.getRawAxis(RightY);
}
else
Turn = 0;
//PERFORMANCE CURVE LOGIC GOES HERE
if(XBoxController.getRawButton(3))
{
Turn *= 2;
}
else
{
Turn *= 0.85;
}
return Turn;
}
{
/*
1: A
2: B
3: X
4: Y
5: Left Bumper
6: Right Bumper
7: Back
8: Start
9: Left Joystick
10: Right Joystick
The axis on the controller follow this mapping
(all output is between -1 and 1)
1: Left Stick X Axis
-Left:Negative ; Right: Positive
2: Left Stick Y Axis
-Up: Negative ; Down: Positive
3: Triggers
-Left: Positive ; Right: Negative
4: Right Stick X Axis
-Left: Negative ; Right: Positive
5: Right Stick Y Axis
-Up: Negative ; Down: Positive
6: Directional Pad (Not recommended, buggy)
*/
}
public void robotInit()
{ //REMEMBER TO INITIALIZE EVERYTHING
drivetrain = new RobotDrive(1, 3, 2, 4);
XBoxController = new Joystick(1);
gyro = new Gyro(1); // FIND CORRECT INPUT FOR THIS
timer = new Timer();
encoder = new Encoder(1,2); //FIND CORRECT INPUT
}
public void autonomous()
{
while(isAutonomous()&&isEnabled()){ //Makes sure in autonomous.
/*startTimer();
Timer.delay(5);
Timer test double time = restartTimer();
log("Time was"+time);
*/
/* int a = 1;
int b = 1;
do{ encoder = new Encoder(a,b);
Encoder test//encoder.start();
drivetrain.drive(.5,0);
Timer.delay(4);
encoder.stop();
double count = encoder.get();
double distance = encoder.getDistance();
boolean direction = encoder.getDirection();
log("Count is "+count+" distance is "+distance+" direction is "+direction+ "ON aSlot = "+a+" and bSlot = "+b);
}while(a<10 && b<10)
do{ a = 1;
b = 2;
encoder = new Encoder(a,b);
encoder.start();
drivetrain.drive(.5,0);
Timer.delay(4);
encoder.stop();
double count = encoder.get();
double distance = encoder.getDistance();
boolean direction = encoder.getDirection();
log("Count is "+count+" distance is "+distance+" direction is "+direction+ "ON aSlot = "+a+" and bSlot = "+b);
}while(a<10 && b<10)
do{ a = 2;
b = 1;
encoder = new Encoder(a,b);
encoder.start();
drivetrain.drive(.5,0);
Timer.delay(4);
encoder.stop();
double count = encoder.get();
double distance = encoder.getDistance();
boolean direction = encoder.getDirection();
log("Count is "+count+" distance is "+distance+" direction is "+direction+ "ON aSlot = "+a+" and bSlot = "+b);
}while(a<10 && b<10)
*/
/*startTimer();
Motor test while(getTimer()<5){
drivetrain.drive(.5,0);
}
*/
/*gyro.reset();
Gyro test double angle = gyro.getAngle();
log("Angle is "+angle);
*/
}
}
public void operatorControl()
{
while (isOperatorControl() && isEnabled())
{ //Make sure in driver
getEmergency();
while(getBrake()==false)
{
float leftPower;
float rightPower;
leftPower = returnTurn('L');
rightPower = returnTurn('R');
drivetrain.tankDrive(leftPower,rightPower);
}
}
}
}