diff --git a/src/main/java/frc/team7242/robot/Robot.java b/src/main/java/frc/team7242/robot/Robot.java index 5257432..e0c779f 100644 --- a/src/main/java/frc/team7242/robot/Robot.java +++ b/src/main/java/frc/team7242/robot/Robot.java @@ -1,124 +1,94 @@ -/*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ -/* Open Source Software - may be modified and shared by FRC teams. The code */ -/* must be accompanied by the FIRST BSD license file in the root directory of */ -/* the project. */ -/*----------------------------------------------------------------------------*/ - package frc.team7242.robot; import edu.wpi.first.wpilibj.IterativeRobot; -import edu.wpi.first.wpilibj.Joystick; -import edu.wpi.first.wpilibj.Spark; -import edu.wpi.first.wpilibj.Victor; -import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.*; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.team7242.robot.subsystem.Drivetrain; - -/** - * The VM is configured to automatically run this class, and to call the - * functions corresponding to each mode, as described in the IterativeRobot - * documentation. If you change the name of this class or the package after - * creating this project, you must also update the build.properties file in the - * project. - */ +import edu.wpi.first.wpilibj.Spark; +import frc.team7242.robot.subsystem.ScalableSpeedControllerGroup; +import edu.wpi.first.wpilibj.CameraServer; + + public class Robot extends IterativeRobot { private static final String kDefaultAuto = "Default"; private static final String kCustomAuto = "My Auto"; private String m_autoSelected; private SendableChooser m_chooser = new SendableChooser<>(); - Joystick driverStick = new Joystick(0); - XboxController xboxdriver = new XboxController(1); //xbox controller in port 1 - - Drivetrain drivetrain = new Drivetrain(); - - Spark leftFront = new Spark(0); - Spark rightFront = new Spark(1); - Spark leftBack = new Spark(2); - Spark rightBack = new Spark(3); - - double autonomousSpeed = 0.5; - double autonomousTime = 7; - + + double autonomousTime = 3; + double autonomousStartTime; - - /** - * This function is run when the robot is first started up and should be - * used for any initialization code. - */ - @Override + + ScalableSpeedControllerGroup left = new ScalableSpeedControllerGroup(0.75, new Spark(0), new Spark(1)); + ScalableSpeedControllerGroup right = new ScalableSpeedControllerGroup(0.72, new Spark(2), new Spark(3)); + + DifferentialDrive drive = new DifferentialDrive(left, right); + + public void robotInit() { m_chooser.addDefault("Default Auto", kDefaultAuto); m_chooser.addObject("My Auto", kCustomAuto); SmartDashboard.putData("Auto choices", m_chooser); + CameraServer.getInstance().startAutomaticCapture(); + } - /** - * This autonomous (along with the chooser code above) shows how to select - * between different autonomous modes using the dashboard. The sendable - * chooser code works with the Java SmartDashboard. If you prefer the - * LabVIEW Dashboard, remove all of the chooser code and uncomment the - * getString line to get the auto name from the text box below the Gyro - * - *

You can add additional auto modes by adding additional comparisons to - * the switch structure below with additional strings. If using the - * SendableChooser make sure to add them to the chooser code above as well. - */ - @Override public void autonomousInit() { m_autoSelected = m_chooser.getSelected(); - // autoSelected = SmartDashboard.getString("Auto Selector", - // defaultAuto); System.out.println("Auto selected: " + m_autoSelected); autonomousStartTime = Timer.getFPGATimestamp(); } - /** - * This function is called periodically during autonomous. - */ - @Override public void autonomousPeriodic() { double deltaTime = Timer.getFPGATimestamp() - autonomousStartTime; - if(deltaTime < autonomousTime){ - drivetrain.drive(autonomousSpeed, 0.0); - }else{ - drivetrain.drive(0.0, 0.0); + if (deltaTime < autonomousTime) { + drive.arcadeDrive(0.8, 0.0); + } else { + drive.arcadeDrive(0.0, 0.0); + } + + + } + + public void teleopPeriodic() { + + double xvalue = driverStick.getX(); + double yvalue = driverStick.getY(); + double boost; + double braking; + + boolean trigger = driverStick.getRawButton(1); + if (trigger) { + boost = 2; + } else { + boost = 1; + } + + boolean brake = driverStick.getRawButton(2); + if (brake){ + braking = 0; + } + else { + braking = 1; } + double sens = 2; + double threshold = 0.1; + double tres; + if((Math.abs(xvalue)