/----------------------------------------------------------------------------/
/* Copyright © 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.robot;
import edu.wpi.first.wpilibj.TimedRobot;
import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.PWMVictorSPX;
import edu.wpi.first.wpilibj.Timer;
import edu.wpi.first.wpilibj.CounterBase.EncodingType;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
import edu.wpi.first.wpilibj.Spark;
import edu.wpi.first.cameraserver.CameraServer;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.XboxController;
import edu.wpi.first.wpilibj.SpeedController;
import edu.wpi.first.wpilibj.SpeedControllerGroup;
import edu.wpi.first.wpilibj.controller.ProfiledPIDController;
import edu.wpi.first.wpilibj.trajectory.TrapezoidProfile;
/**
- The VM is configured to automatically run this class, and to call the
- functions corresponding to each mode, as described in the TimedRobot
- documentation. If you change the name of this class or the package after
- creating this project, you must also update the build.gradle file in the
- project.
*/
public class Robot extends TimedRobot {
private static final String kDefaultAuto = “Default”;
private static final String kCustomAuto = “My Auto”;
private String m_autoSelected;
private final SendableChooser m_chooser = new SendableChooser<>();
//Drive Motors
private final SpeedController MotorfntL = new Spark(0);
private final SpeedController MotorbckL = new Spark(1);
private final SpeedController MotorfntR = new Spark(2);
private final SpeedController MotorbckR = new Spark(3);
private final SpeedControllerGroup DriveL = new SpeedControllerGroup(MotorfntL, MotorbckL);
private final SpeedControllerGroup DriveR = new SpeedControllerGroup(MotorfntR, MotorbckR);
private final DifferentialDrive Drivebase = new DifferentialDrive(DriveL, DriveR);
// ARM
private final SpeedController ArmL = new PWMVictorSPX(4);
private final SpeedController ArmR = new PWMVictorSPX(5);
private final SpeedControllerGroup ARM = new SpeedControllerGroup(ArmL, ArmR);
// INTAKE
private final SpeedController IntakeT = new PWMVictorSPX(6);
private final SpeedController IntakeB = new PWMVictorSPX(7);
private final SpeedControllerGroup INTAKE = new SpeedControllerGroup(IntakeT, IntakeB);
//SHOOTER
private final SpeedController ShooterL = new PWMVictorSPX(8);
private final SpeedController ShooterR = new PWMVictorSPX(9);
private final SpeedControllerGroup SHOOTER = new SpeedControllerGroup(ShooterL,ShooterR);
//PID
private final TrapezoidProfile.Constraints m_constraints = new TrapezoidProfile.Constraints(.5, 0.25);
private final ProfiledPIDController PID = new ProfiledPIDController(.3, 6.5, 0, m_constraints);
//ENCODER
private final Encoder ENC = new Encoder(0,0,false,EncodingType.k4X);
//Controllers
private final Joystick Joy = new Joystick(0);
private final XboxController Xbox = new XboxController(1);
//Variables, doubles,integers, and booleans
private final double AnalogToDegrees = .3515625;
private final double ShaftAngle =(ENC.get()*0.3515625);
private final double EncRaw =ENC.get();
private final double SlowUp = .2;
private final double SlowDown = -.2;
private final double updown = Joy.getPOV(0);
private final boolean isinverted =(true);
private final boolean ispressed = (true);
private final int RightTrigger = 1;
private final int RightStick_X = 5;
/**
- This function is run when the robot is first started up and should be
- used for any initialization code.
*/
@Override
public void robotInit() {
m_chooser.setDefaultOption(“Default Auto”, kDefaultAuto);
m_chooser.addOption(“My Auto”, kCustomAuto);
SmartDashboard.putData(“Auto choices”, m_chooser);
CameraServer.getInstance().startAutomaticCapture();
CameraServer.getInstance().startAutomaticCapture();
SmartDashboard.putNumber(“Encoder Raw”, EncRaw);
SmartDashboard.putNumber(“shaft Angle”, ShaftAngle);
ENC.setDistancePerPulse(AnalogToDegrees);
ArmL.setInverted(isinverted);
}
/**
/**
/**
- This function is called periodically during autonomous.
*/
@Override
public void autonomousPeriodic() {
switch (m_autoSelected) {
case kCustomAuto:
// Put custom auto code here
break;
case kDefaultAuto:
default:
// Put default auto code here
break;
}
}
/**
- This function is called periodically during operator control.
*/
@Override
public void teleopPeriodic() {
while (isOperatorControl() && isEnabled() ) {
Drive();
ArmPositions();
ArmManual();
Intake();
Shooter();
WheelSpin();
Timer.delay(.05);
}}
public void Drive() {
Drivebase.arcadeDrive(-Joy.getRawAxis(1), Joy.getRawAxis(0));
}
public void ArmPositions() {
if (Joy.getRawButton(1) == ispressed) { PID.setGoal(70);
ARM.set(PID.calculate(ENC.getDistance())); }
if (Joy.getRawButton(2) == ispressed) { PID.setGoal(30);
ARM.set(PID.calculate(ENC.getDistance())); }
if (Joy.getRawButton(3) == ispressed) { PID.setGoal(10);
ARM.set(PID.calculate(ENC.getDistance())); }
}
public void ArmManual() {
if (updown == (0)) {ARM.set(SlowUp);}
if (updown == (180)) {ARM.set(SlowDown);}
double output = Joy.getY(); //Moves the Joystick based on Y value
if (ENC.get() < 10) // If the lower soft limit switch is reached, we want to keep the values between -1 and 0
output = Math.min(output, 0);
else if(ENC.get() > 500) // If the higher soft limit switch is reached, we want to keep the values between 0 and 1
output = Math.max(output, 0);
ARM.set(output);
}
public void Intake() {
while(Xbox.getRawAxis(1) <= (0.5) && Xbox.getRawAxis(1) >= (-.5))
{Double IntakeSpeed = Xbox.getRawAxis(1);
INTAKE.set(IntakeSpeed);
}
while(Xbox.getRawAxis(1) > (0.5))
{Double IntakeSpeed = .5;
INTAKE.set(IntakeSpeed);
}
while(Xbox.getRawAxis(1) < (-0.5))
{Double IntakeSpeed = -0.5;
INTAKE.set(IntakeSpeed);
}
}
public void Shooter() {
SHOOTER.set(Xbox.getRawAxis(RightTrigger));
}
public void WheelSpin() {
if (Joy.getRawButton(2) == ispressed) { PID.setGoal(35);
ARM.set(PID.calculate(ENC.getDistance())); }
ShooterL.set(-Xbox.getRawAxis(RightStick_X));
ShooterR.set(Xbox.getRawAxis(RightStick_X));
}
//https://github.com/wpilibsuite/allwpilib/blob/master/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/elevatorprofiledpid/Robot.java
//https://first.wpi.edu/FRC/roborio/release/docs/java/index.html
/**
- This function is called periodically during test mode.
*/
@Override
public void testPeriodic() {
}
}
