diff --git a/build.gradle b/build.gradle index 9128ffc..abdb212 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2019.1.1" + id "edu.wpi.first.GradleRIO" version "2019.3.2" } def ROBOT_MAIN_CLASS = "frc.robot.Main" diff --git a/src/main/java/frc/robot/OI.java b/src/main/java/frc/robot/OI.java index 5d57329..3529bb0 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -7,6 +7,7 @@ package frc.robot; +import edu.wpi.first.wpilibj.Joystick; /** * This class is the glue that binds the controls on the physical operator * interface to the commands and command groups that allow control of the robot. @@ -20,12 +21,16 @@ public class OI { // Joystick stick = new Joystick(port); // Button button = new JoystickButton(stick, buttonNumber); + public static Joystick joyLeft = new Joystick(0); // port subject to change + public static Joystick joyRight = new Joystick(1); + + // There are a few additional built in buttons you can use. Additionally, // by subclassing Button you can create custom triggers and bind those to // commands the same as any other Button. //// TRIGGERING COMMANDS WITH BUTTONS - // Once you have a button, it's trivial to bind it to a button in one of + // Once you have a button, it's trivial to bind it to a button in oneP of // three ways: // Start the command when the button is pressed and let it run the command @@ -34,9 +39,7 @@ public class OI { // Run the command while the button is being held down and interrupt it once // the button is released. - // button.whileHeld(new ExampleCommand()); - - // Start the command when the button is released and let it run the command - // until it is finished as determined by it's isFinished method. - // button.whenReleased(new ExampleCommand()); + public OI() { + + } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index 0fc498a..80d776e 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -12,8 +12,10 @@ import edu.wpi.first.wpilibj.command.Scheduler; import edu.wpi.first.wpilibj.smartdashboard.SendableChooser; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.commands.ExampleCommand; -import frc.robot.subsystems.ExampleSubsystem; +import frc.robot.subsystems.EncoderTest; +import frc.robot.subsystems.Gyroscope; +import frc.robot.subsystems.PIDControl; +import frc.robot.subsystems.TankDrive; /** * The VM is configured to automatically run this class, and to call the @@ -23,11 +25,14 @@ * project. */ public class Robot extends TimedRobot { - public static ExampleSubsystem m_subsystem = new ExampleSubsystem(); - public static OI m_oi; + public static TankDrive tankDriveSubsystem = new TankDrive(); + public static EncoderTest encoderSub = new EncoderTest(); + public static Gyroscope gyroSub = new Gyroscope(); + public static PIDControl pidSub = new PIDControl(10);//place holder for distance + public static OI oi; - Command m_autonomousCommand; - SendableChooser m_chooser = new SendableChooser<>(); + Command autonomousCommand; + SendableChooser chooser = new SendableChooser<>(); /** * This function is run when the robot is first started up and should be @@ -35,10 +40,9 @@ public class Robot extends TimedRobot { */ @Override public void robotInit() { - m_oi = new OI(); - m_chooser.setDefaultOption("Default Auto", new ExampleCommand()); + oi = new OI(); // chooser.addOption("My Auto", new MyAutoCommand()); - SmartDashboard.putData("Auto mode", m_chooser); + SmartDashboard.putData("Auto mode", chooser); } /** @@ -80,7 +84,7 @@ public void disabledPeriodic() { */ @Override public void autonomousInit() { - m_autonomousCommand = m_chooser.getSelected(); + autonomousCommand = chooser.getSelected(); /* * String autoSelected = SmartDashboard.getString("Auto Selector", @@ -90,8 +94,8 @@ public void autonomousInit() { */ // schedule the autonomous command (example) - if (m_autonomousCommand != null) { - m_autonomousCommand.start(); + if (autonomousCommand != null) { + autonomousCommand.start(); } } @@ -109,8 +113,8 @@ public void teleopInit() { // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove // this line or comment it out. - if (m_autonomousCommand != null) { - m_autonomousCommand.cancel(); + if (autonomousCommand != null) { + autonomousCommand.cancel(); } } diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 45360e4..48826fb 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -7,6 +7,8 @@ package frc.robot; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; + /** * The RobotMap is a mapping from the ports sensors and actuators are wired into * to a variable name. This provides flexibility changing wiring, makes checking @@ -23,4 +25,6 @@ public class RobotMap { // number and the module. For example you with a rangefinder: // public static int rangefinderPort = 1; // public static int rangefinderModule = 1; + + public static WPI_TalonSRX TANK_DRIVE_TOP_LEFT = new WPI_TalonSRX(15); //port # not yet known? to be attached with encoder } diff --git a/src/main/java/frc/robot/commands/ExampleCommand.java b/src/main/java/frc/robot/commands/AutonDriveCommand.java similarity index 78% rename from src/main/java/frc/robot/commands/ExampleCommand.java rename to src/main/java/frc/robot/commands/AutonDriveCommand.java index 83a3fcc..d6efa45 100644 --- a/src/main/java/frc/robot/commands/ExampleCommand.java +++ b/src/main/java/frc/robot/commands/AutonDriveCommand.java @@ -1,5 +1,5 @@ /*----------------------------------------------------------------------------*/ -/* Copyright (c) 2017-2018 FIRST. All Rights Reserved. */ +/* Copyright (c) 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. */ @@ -10,13 +10,10 @@ import edu.wpi.first.wpilibj.command.Command; import frc.robot.Robot; -/** - * An example command. You can replace me with your own command. - */ -public class ExampleCommand extends Command { - public ExampleCommand() { +public class AutonDriveCommand extends Command { // yet to be tested + public AutonDriveCommand() { // Use requires() here to declare subsystem dependencies - requires(Robot.m_subsystem); + requires(Robot.pidSub); } // Called just before this Command runs the first time @@ -32,7 +29,7 @@ protected void execute() { // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - return false; + return Robot.pidSub.encoderPID.onTarget(); // stop when PID reads that we've met designated position. onTarget() will == true } // Called once after isFinished returns true diff --git a/src/main/java/frc/robot/commands/DriveCommand.java b/src/main/java/frc/robot/commands/DriveCommand.java new file mode 100644 index 0000000..d63c621 --- /dev/null +++ b/src/main/java/frc/robot/commands/DriveCommand.java @@ -0,0 +1,50 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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.commands; + +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Robot; + +public class DriveCommand extends Command { // yet to be tested + public DriveCommand() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.tankDriveSubsystem); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + Robot.tankDriveSubsystem.init(); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + Robot.tankDriveSubsystem.driveWithJoystick(); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return false; + } + + // Called once after isFinished returns true + @Override + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + Robot.tankDriveSubsystem.stop(); + } +} diff --git a/src/main/java/frc/robot/commands/EncoderCommand.java b/src/main/java/frc/robot/commands/EncoderCommand.java new file mode 100644 index 0000000..88f4560 --- /dev/null +++ b/src/main/java/frc/robot/commands/EncoderCommand.java @@ -0,0 +1,41 @@ +package frc.robot.commands; + +import frc.robot.Robot; +import frc.robot.RobotMap; +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + +public class EncoderCommand extends Command { // yet to be tested + public EncoderCommand() { + requires(Robot.encoderSub); + } + // Called just before this Command runs the first time + @Override + protected void initialize() { + RobotMap.TANK_DRIVE_TOP_LEFT.setSelectedSensorPosition(0); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + SmartDashboard.putNumber("Position", Robot.encoderSub.getPosition()); + Robot.encoderSub.goStraight(10); //Parameter is a placeholder = 10 inch to move + } + + @Override + protected boolean isFinished() { + return false; + } + + // Called once after isFinished returns true + @Override + protected void end() { + + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/commands/GoToAngle.java b/src/main/java/frc/robot/commands/GoToAngle.java new file mode 100644 index 0000000..9f8521a --- /dev/null +++ b/src/main/java/frc/robot/commands/GoToAngle.java @@ -0,0 +1,60 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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.commands; + +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.Robot; + +public class GoToAngle extends Command { //yet to be tested + + public double finalAngle; + public double finalAngle1; + public double specifiedAngle; + + public GoToAngle(double pfinalAngle1, double pfinalAngle2, double pSpecifiedAngle) { + requires(Robot.gyroSub); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + Robot.gyroSub.gyro.reset(); + Robot.tankDriveSubsystem.driveToAngle(specifiedAngle); //autonomous? + //Robot.gyroSub.motor.set(0.5); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + SmartDashboard.putNumber("Current angle ", Robot.gyroSub.gyro.getAngle()); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + SmartDashboard.putBoolean("finished", (Robot.gyroSub.gyro.getAngle() >= finalAngle && Robot.gyroSub.gyro.getAngle() <= finalAngle1)); + return (Robot.gyroSub.gyro.getAngle() >= finalAngle && Robot.gyroSub.gyro.getAngle() <= finalAngle1); + } + + // Called once after isFinished returns true + @Override + protected void end() { + Robot.tankDriveSubsystem.stop(); + Robot.gyroSub.stop(); + + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + Robot.tankDriveSubsystem.stop(); + Robot.gyroSub.stop(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/EncoderTest.java b/src/main/java/frc/robot/subsystems/EncoderTest.java new file mode 100644 index 0000000..278acb9 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/EncoderTest.java @@ -0,0 +1,49 @@ +package frc.robot.subsystems; + +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced; + +import edu.wpi.first.wpilibj.command.Subsystem; +import frc.robot.Robot; +import frc.robot.RobotMap; + +public class EncoderTest extends Subsystem { // yet to be tested + + public EncoderTest() { + RobotMap.TANK_DRIVE_TOP_LEFT.setSensorPhase(true); + RobotMap.TANK_DRIVE_TOP_LEFT.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, 1, 10); + RobotMap.TANK_DRIVE_TOP_LEFT.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, 0, 10); + } + + public double getPosition() { + return (double)RobotMap.TANK_DRIVE_TOP_LEFT.getSelectedSensorPosition(); // where the robot is + } + + public void goStraight(double distance) { //where 6 is the circumference of the wheels and distance is in inches + double rotations = distance / (6 * Math.PI); //gets the number of times the wheel needs to rotate to reach a specified distance (inch) (rotations = distance/circumference) + double units = 400 * rotations; // gets the total number of raw sensor units correlated with specified distance + Robot.tankDriveSubsystem.driveForward(); //Drives with 0.5 speed + if (getPosition() >= units) //determines if the raw sensor units thus far equates to that of specified distance. if so, stop moving + Robot.tankDriveSubsystem.stop(); //stops all motors + } + + public void turnToAngle(double distance, double angle) { + double rotations = distance / (6 * Math.PI); //gets the number of times the wheel needs to rotate to reach a specified distance (in) (rotations = distance/circumference) + double units = 400 * rotations; // gets the total number of raw sensor units correlated with specified distance + Robot.tankDriveSubsystem.driveToAngle(angle); //Drives with 0.5 speed + if (getPosition() >= units) //determines if the raw sensor units thus far equates to that of specified distance. if so, stop moving + Robot.tankDriveSubsystem.stop(); //stops all motors + } + + public void turnToAngle(double angleOfTurn) { + double circumferenceOfRobot = Math.PI * 22.74; //units are in inches, where 22.74 is a measure of the distance between each wheel + double distance = (double)(circumferenceOfRobot * (angleOfTurn/360)); //gets the distance needed to turn a specified angle + turnToAngle(distance, angleOfTurn); // calls on auton tank drive for turning to angles + } + + @Override + protected void initDefaultCommand() { + //setDefaultCommand(new EncoderCommand()); + } + +} diff --git a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java b/src/main/java/frc/robot/subsystems/ExampleSubsystem.java deleted file mode 100644 index 7f5e1a4..0000000 --- a/src/main/java/frc/robot/subsystems/ExampleSubsystem.java +++ /dev/null @@ -1,24 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* 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.robot.subsystems; - -import edu.wpi.first.wpilibj.command.Subsystem; - -/** - * An example subsystem. You can replace me with your own Subsystem. - */ -public class ExampleSubsystem extends Subsystem { - // Put methods for controlling this subsystem - // here. Call these from Commands. - - @Override - public void initDefaultCommand() { - // Set the default command for a subsystem here. - // setDefaultCommand(new MySpecialCommand()); - } -} diff --git a/src/main/java/frc/robot/subsystems/Gyroscope.java b/src/main/java/frc/robot/subsystems/Gyroscope.java new file mode 100644 index 0000000..5468981 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Gyroscope.java @@ -0,0 +1,34 @@ +package frc.robot.subsystems; + +import edu.wpi.first.wpilibj.ADXRS450_Gyro; +import edu.wpi.first.wpilibj.command.Subsystem; +import frc.robot.Robot; +import frc.robot.commands.GoToAngle; + +public class Gyroscope extends Subsystem { //yet to be tested + + int count = 0; + public double finalAngle; + public double finalAngle1; + public double specifiedAngle; + public ADXRS450_Gyro gyro = new ADXRS450_Gyro(); + + //public WPI_TalonSRX motor = new WPI_TalonSRX(11); + + public Gyroscope() { + gyro.calibrate(); + } + + public void reset() { + gyro.reset(); + } + + @Override + public void initDefaultCommand() { + //setDefaultCommand(new GoToAngle(10,11,10)); + } + public void stop() { + Robot.gyroSub.stop(); + Robot.tankDriveSubsystem.stop(); + } +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/PIDControl.java b/src/main/java/frc/robot/subsystems/PIDControl.java new file mode 100644 index 0000000..c883f7f --- /dev/null +++ b/src/main/java/frc/robot/subsystems/PIDControl.java @@ -0,0 +1,96 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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.subsystems; + +import edu.wpi.first.wpilibj.command.PIDSubsystem; +import frc.robot.Robot; +import frc.robot.commands.AutonDriveCommand; +import frc.robot.commands.DriveCommand; + +import com.ctre.phoenix.motorcontrol.FeedbackDevice; +import com.ctre.phoenix.motorcontrol.StatusFrameEnhanced; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; + +import edu.wpi.first.wpilibj.PIDController; + +/** + * Add your docs here. + */ +public class PIDControl extends PIDSubsystem { //yet to be tested + /** + * Add your docs here. + */ + public PIDController encoderPID = getPIDController(); + + public WPI_TalonSRX talon = new WPI_TalonSRX(15); + public double dis; + + public PIDControl(double distance) {//in inches { + // Insert a subsystem name and PID values here + super("EncoderTest", 0, 0, 0); + talon.setSensorPhase(true); + talon.setStatusFramePeriod(StatusFrameEnhanced.Status_2_Feedback0, 1, 10); + talon.configSelectedFeedbackSensor(FeedbackDevice.QuadEncoder, 0, 10); + dis = distance; + double rotations = distance / (6 * Math.PI); + double units = 360 * rotations; //encoder units + encoderPID.setSetpoint(units); + encoderPID.setPercentTolerance(5); + encoderPID.enable(); // Enables the PID controller. + } + + @Override + public void initDefaultCommand() { + // Set the default command for a subsystem here. + setDefaultCommand(new AutonDriveCommand()); + } + + public double getPosition() { + return (double)talon.getSelectedSensorPosition(); + } + + public void goStraight() { //where 6 is the diameter of the wheels and distance is in in + Robot.tankDriveSubsystem.driveForward(); //Drives with 0.5 speed + if (encoderPID.onTarget()) { + Robot.tankDriveSubsystem.stop(); + } + } + + public void turnToAngle(double distance, double angle) { //where 15.24 is the circumference of the wheels and distance is in in + double rotations = distance / (6 * Math.PI); //gets the number of times the wheel needs to rotate to reach a specified distance (in) (rotations = distance/circumference) + double units = 360 * rotations; // gets the total number of raw sensor units correlated with specified distance + Robot.tankDriveSubsystem.driveToAngle(angle); //Drives with 0.5 speed + if (getPosition() >= units) //determines if the raw sensor units thus far equates to that of specified distance. if so, stop moving + Robot.tankDriveSubsystem.stop(); //stops all motors + } + + public void turnToAngle(double angleOfTurn) { /* new edits */ + double circumferenceOfRobot = 57.76; //units are in centimeter + double distance = (double)(circumferenceOfRobot * (angleOfTurn/360)); //gets the distance needed to turn a specified angle + turnToAngle(distance, angleOfTurn);; + } + + public void reset() { + encoderPID.reset(); + } + + @Override + protected double returnPIDInput() { + // Return your input value for the PID loop + // e.g. a sensor, like a potentiometer: + // yourPot.getAverageVoltage() / kYourMaxVoltage; + return talon.getSelectedSensorPosition(0); + } + + @Override + protected void usePIDOutput(double output) { + // Use output to drive your system, like a motor + // e.g. yourMotor.set(output); + talon.pidWrite(output); + } +} diff --git a/src/main/java/frc/robot/subsystems/TankDrive.java b/src/main/java/frc/robot/subsystems/TankDrive.java new file mode 100644 index 0000000..326d088 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/TankDrive.java @@ -0,0 +1,130 @@ +/*----------------------------------------------------------------------------*/ +/* Copyright (c) 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.subsystems; + +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; + +import edu.wpi.first.wpilibj.SpeedControllerGroup; +import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.commands.DriveCommand; +import frc.robot.OI; + +public class TankDrive extends Subsystem { + // Put methods for controlling this subsystem + // here. Call these from Commands. + + public WPI_TalonSRX topLeft = new WPI_TalonSRX(3); + public WPI_TalonSRX middleLeft = new WPI_TalonSRX(4); + public WPI_TalonSRX rearLeft = new WPI_TalonSRX(10); + + SpeedControllerGroup leftSide = new SpeedControllerGroup(topLeft, middleLeft, rearLeft); // controls the left side motors of tank drive + + public WPI_TalonSRX topRight = new WPI_TalonSRX(5); + public WPI_TalonSRX middleRight = new WPI_TalonSRX(13); + public WPI_TalonSRX rearRight = new WPI_TalonSRX(1); + + SpeedControllerGroup rightSide = new SpeedControllerGroup(topRight, middleRight, rearRight); //controls the right side motors of tank drive + + DifferentialDrive difDrive = new DifferentialDrive(leftSide, rightSide); + + @Override + public void initDefaultCommand() { + // Set the default command for a subsystem here. + // setDefaultCommand(new MySpecialCommand()); + setDefaultCommand(new DriveCommand()); + } + + public void init() { // middleLeft and middleRight motor must go in opposite directions from the rest of the motors. + middleLeft.follow(topLeft); + rearLeft.follow(topLeft); + + middleLeft.setInverted(true); // inverted reverses the direction = goes in the opposite direction + rearLeft.setInverted(true); + + middleRight.follow(topRight); + rearRight.follow(topRight); + + middleRight.setInverted(true); + rearRight.setInverted(true); + } + + public void driveWithJoystick() { + double leftSpeed = OI.joyLeft.getY() * 0.8; // motors cannot exceed 80% + double rightSpeed = OI.joyRight.getY() * 0.8; + + if(Math.abs(leftSpeed) < 0.2) + leftSpeed = 0; + + if(Math.abs(rightSpeed) < 0.2) + rightSpeed = 0; + + /*if(Math.abs(leftSpeed) > 0.8) + leftSpeed = 0.8; + + if(Math.abs(rightSpeed) > 0.8) + rightSpeed = 0.8;*/ + + SmartDashboard.putNumber("Front Left Motor:", topLeft.get()); + SmartDashboard.putNumber("Middle Left Motor:", middleLeft.get()); + SmartDashboard.putNumber("Rear Left Motor:", rearLeft.get()); + + SmartDashboard.putNumber("Front Right Motor:", topRight.get()); + SmartDashboard.putNumber("Middle Right Motor:", middleRight.get()); + SmartDashboard.putNumber("Rear Right Motor:", rearRight.get()); + + difDrive.tankDrive(leftSpeed, rightSpeed); + } + + public void driveForward() { + double leftSpeed = 0.5; + double rightSpeed = -0.5; + difDrive.tankDrive(leftSpeed, rightSpeed); + + SmartDashboard.putNumber("Front Left Motor:", topLeft.get()); + SmartDashboard.putNumber("Middle Left Motor:", middleLeft.get()); + SmartDashboard.putNumber("Rear Left Motor:", rearLeft.get()); + + SmartDashboard.putNumber("Front Right Motor:", topRight.get()); + SmartDashboard.putNumber("Middle Right Motor:", middleRight.get()); + SmartDashboard.putNumber("Rear Right Motor:", rearRight.get()); + } + + public void driveToAngle(double angle) { + double leftSpeed, rightSpeed; + //the speedcontroller changes depending on what angle + if(angle < 180.0) { //denotes it turning right. left side tank drive speed will be faster. + leftSpeed = 0.6; + rightSpeed = 0.5; + } + else { //denotes it turning left. right side tank drive will be faster + rightSpeed = 0.6; + leftSpeed = 0.5; + } + difDrive.tankDrive(leftSpeed, rightSpeed); + + SmartDashboard.putNumber("Front Left Motor:", topLeft.get()); + SmartDashboard.putNumber("Middle Left Motor:", middleLeft.get()); + SmartDashboard.putNumber("Rear Left Motor:", rearLeft.get()); + + SmartDashboard.putNumber("Front Right Motor:", topRight.get()); + SmartDashboard.putNumber("Middle Right Motor:", middleRight.get()); + SmartDashboard.putNumber("Rear Right Motor:", rearRight.get()); + } + + public void stop() { + topLeft.stopMotor(); + middleLeft.stopMotor(); + rearLeft.stopMotor(); + + topRight.stopMotor(); + middleRight.stopMotor(); + rearRight.stopMotor(); + } + +} diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json new file mode 100644 index 0000000..a1654ec --- /dev/null +++ b/vendordeps/Phoenix.json @@ -0,0 +1,87 @@ +{ + "fileName": "Phoenix.json", + "name": "CTRE-Phoenix", + "version": "5.12.1", + "uuid": "ab676553-b602-441f-a38d-f1296eff6537", + "mavenUrls": [ + "http://devsite.ctr-electronics.com/maven/release/" + ], + "jsonUrl": "http://devsite.ctr-electronics.com/maven/release/com/ctre/phoenix/Phoenix-latest.json", + "javaDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-java", + "version": "5.12.1" + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-java", + "version": "5.12.1" + } + ], + "jniDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.12.1", + "isJar": false, + "skipInvalidPlatforms": true, + "validPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + } + ], + "cppDependencies": [ + { + "groupId": "com.ctre.phoenix", + "artifactId": "wpiapi-cpp", + "version": "5.12.1", + "libName": "CTRE_Phoenix_WPI", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "api-cpp", + "version": "5.12.1", + "libName": "CTRE_Phoenix", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "cci", + "version": "5.12.1", + "libName": "CTRE_PhoenixCCI", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena", + "windowsx86-64", + "linuxx86-64" + ] + }, + { + "groupId": "com.ctre.phoenix", + "artifactId": "core", + "version": "5.12.1", + "libName": "CTRE_PhoenixCore", + "headerClassifier": "headers" + } + ] +} \ No newline at end of file