/*
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 joystick;
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 String log(String theMessage){ // Logger -System.out.println function
System.out.println(theMessage);
return theMessage;
}
public void BRAKE(){ //Brake method - stops motors
drivetrain.drive(0.0,0.0);
drivetrain.stopMotor();
}
public void EMERGENCY(){ //ADD ALL FUNCTIONS HERE TO STOP ON ROBOT DURING EMERGENCY
drivetrain.drive(0.0,0.0);
Timer.delay(10.0);
drivetrain.stopMotor();
EMERGENCY();
}
public void RESET(){
gyro.reset();
timer.reset();
drivetrain.stopMotor();
}
public void startTimer(){
timer.start();
}
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 returnSpeed(){
double speed = XBoxController.getRawAxis(Triggers);
if (speed > 1){
speed = 1;
}else if (speed < -1){
speed = -1;
}else {
speed = 0;
}
return speed;
}
public double getTimer(){
double time = timer.get();
return time;
}
public double restartTimer(){
double time = timer.get();
timer.stop();
timer.reset();
timer.start();
return time;
}
public float returnTurn(){
float Turn = 0;
if(XBoxController.getRawAxis(RightX)>=0.15){
Turn = (float)XBoxController.getRawAxis(RightX);
}
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, 2, 3, 4);
joystick = new Joystick(1);
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();
if(getBrake()==false){
double speed =returnSpeed();
float turn =returnTurn();
if((turn > -1 && turn < 1 && turn != 0) && speed == 0 ){
speed = turn ;
}
drivetrain.drive(speed,turn);
}
}
}
}