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 Button button; // 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);
}
public void autonomous(){
while(isAutonomous()&&isEnabled()){ //Make sure in autonomous
timer.start(); //See how long program runs
gyro.reset();
int i=0;
while(i<4){
drivetrain.drive(0.5,0.0); //START GOING STRAIGHT
Timer.delay(1.5);
drivetrain.drive(0.0,0.0); //STOP
Timer.delay(0.5);
gyro.reset();
double angle = 0;
do {
drivetrain.drive(0.0, 0.15); //TURN
Timer.delay(0.5);
angle += gyro.getAngle(); //
gyro.reset();
log("Gyro reads "+angle);
}while(angle<90); //Keep going in increments of .15 until gyro returns more than 90 degrees
drivetrain.drive(0.0,0.0); //STOP
Timer.delay(0.5);
i++;
}
if(isAutonomous()&&isEnabled()){
double timerTime = timer.get(); timer.stop(); timer.reset();
log("Done. Autonomous took "+timerTime+" seconds long to complete."); //GET RID OF THIS WHEN IN COMPETITION
}
}
}
public void operatorControl() {
while (isOperatorControl() && isEnabled()){ //Make sure in driver
if(KTrigger.getTrigger())
{drivetrain.drive(0,0);
log("Brake enabled");}
double YAxis = joystick.getAxis(Joystick.AxisType.kY);
double XAxis = joystick.getAxis(Joystick.AxisType.kX);
if(XAxis==0){
XAxis=0.2;
}
drivetrain.drive(YAxis,XAxis);
}
}
// public abstract class Button extends Object implements SmartDashboardData{} //WORKING ON THIS(1)
public static String log(String theMessage){ // LOGGER -System.out.println function
System.out.println(theMessage);
return theMessage;
}
}