/*
THIS CODE IS MADE BY THE BEST PEOPLE IN THE WORLD, DO NOT STEAL.
THINGS TO DO ARE IN CAPS
Instructions are in sentence form.
*/
package edu.wpi.first.wpilibj.templates;
import edu.wpi.first.wpilibj.*;
// import edu.wpi.first.wpilibj.smartdashboard.SmartDashboardData; //WORKING ON THIS(1)
public class Robot4334 extends SimpleRobot {
//DECLARE ALL METHODS IN INIT AND SIMPLEROBOT
public RobotDrive drivetrain;
public Joystick joystick;
public Joystick KTrigger;
public Gyro gyro; //gyro
public Timer timer;
public static final double DRIVE_STRAIGHT = 0;
public static final double DRIVE_FULL_SPEED = 1; //Variable set for driving full speed for ease of use
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);
drivetrain.stopMotor();
}
// public Button button; // WORKING ON THIS(1)
// public abstract class Button extends Object implements SmartDashboardData{} //WORKING ON THIS(1)
public void robotInit(){ //REMEMBER TO INITIALIZE EVERYTHING
drivetrain = new RobotDrive(1, 2, 3, 4);
joystick = new Joystick(1);
KTrigger = new Joystick(1);
gyro = new Gyro(1); // FIND CORRECT INPUT FOR THIS
}
public void autonomous(){
while(isAutonomous()&&isEnabled()){ //Makes sure in autonomous.
timer.start(); //Sees how long program runs.
gyro.reset(); //Sets gyro to 0.
int i=0; //Declare i out of while loop
while(i<4){ //Runs in a square once (4 lines and 4 turns.
drivetrain.drive(0.5,0.0); //Starts going straight for 1.5 seconds.
Timer.delay(1.5);
drivetrain.drive(0.0,0.0); //Stops for .5 seconds.
Timer.delay(0.5);
gyro.reset(); //Sets gyro to 0.
double angle = 0; //Resets angle measurement from last loop (If a bug occured, otherwise it should reset but this is a percaution)
do {
drivetrain.drive(0.05, 0.15); //Turns a small amount.
Timer.delay(0.1);
BRAKE(); //Stops robot for .5 seconds to read the gyro
Timer.delay(0.5);
angle += gyro.getAngle(); //DEDUCT MARGIN OF ERROR - This adds angle moved to variable 'angle' to track how much it has moved
gyro.reset(); //Resets gyro so it can add to 'angle' again
log("Gyro reads "+angle); //Log for testing purposes
}while(angle<90); //Keeps going in increments of .15 until gyro returns more than 90 degrees.
drivetrain.drive(0.0,0.0); //Stops process when at corner facing the right way
Timer.delay(0.5); //Stops for .5 seconds
i++; //Tracks sides of square
}
if(isAutonomous()&&isEnabled()){ //If the code is done and still in autonomous, this will show how long code took to run, and run 1 second delays for 120 seconds.
double timerTime = timer.get(); timer.stop(); timer.reset(); //Gets time, stops and resets timer
log("Done. Autonomous took "+timerTime+" seconds long to complete."); //GET RID OF THIS WHEN IN COMPETITION
int x=1; while(x>=120){ //REMOVE THIS LOOP WHEN ONE-TIME AUTONOMOUS BUG IS FIXED
Timer.delay(1.0);
}
}else {
}
}
}
public void operatorControl() {
while (isOperatorControl() && isEnabled()){ //Make sure in driver
while(KTrigger.getTrigger()){ //FIND OUT HOW TO OVERIDE JOYSTICK INPUT - Brake function.
BRAKE();
log("Brake enabled");
}
while(KTrigger.getTrigger()==false){
while(joystick.getRawButton(5)==true){ //If button 5 is pressed, run at full speed straight
drivetrain.drive(DRIVE_FULL_SPEED,DRIVE_STRAIGHT);
}
double YAxis = joystick.getY();
double XAxis = joystick.getX();
if(YAxis==0 && (XAxis>0 || XAxis<0)){ //Removes dead-zone
log("IN DEAD ZONE"); // REMOVE THIS FOR REAL ROBOT
YAxis=0.1; //FIND GOOD DEFAULT SPEED (10% right now)
}
if(joystick.getRawButton(3)){ //Button 3 is half speed button
YAxis /= 2;
}
YAxis /= joystick.getThrottle(); //Throttle determines speed (Not actual speed, but joystick axis is devided by throttle. ex.throttle =100%, so speed is not changed [1.0*1.0])
drivetrain.drive(YAxis,XAxis);
}
}
}
}