diff --git a/build.gradle b/build.gradle index 2a7ec8c..f4529be 100644 --- a/build.gradle +++ b/build.gradle @@ -1,6 +1,6 @@ plugins { id "java" - id "edu.wpi.first.GradleRIO" version "2019.2.1" + id "edu.wpi.first.GradleRIO" version "2019.4.1" } 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 f5007cc..68e8a02 100644 --- a/src/main/java/frc/robot/OI.java +++ b/src/main/java/frc/robot/OI.java @@ -7,13 +7,19 @@ package frc.robot; +import java.awt.event.KeyListener; +import java.util.ResourceBundle.Control; + +import edu.wpi.first.wpilibj.Controller; +import edu.wpi.first.wpilibj.Joystick; import edu.wpi.first.wpilibj.XboxController; import edu.wpi.first.wpilibj.buttons.Button; import edu.wpi.first.wpilibj.buttons.JoystickButton; -import frc.robot.commands.ArmToNextPosition; -import frc.robot.commands.ClawReverse; -import frc.robot.commands.MoveClaw; -import frc.robot.commands.SetSpeed; +import edu.wpi.first.wpilibj.buttons.Trigger; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.commands.*; +import frc.robot.subsystems.Arm; +import frc.robot.subsystems.CameraI2c; /** * This class is the glue that binds the controls on the physical operator @@ -49,19 +55,44 @@ public class OI { // button.whenReleased(new ExampleCommand()); public static XboxController xBoxControl = new XboxController(0); - + public static Joystick xBoxControlArm = new Joystick(1); + Button pistonExtend = new JoystickButton(xBoxControl, 8); + Button pistonRetract = new JoystickButton(xBoxControl, 7); + Button lowRocket_c = new JoystickButton(xBoxControlArm, 1); + Button midRocket_c = new JoystickButton(xBoxControlArm, 2); + Button button = new JoystickButton(xBoxControl, 2); + Button climb = new JoystickButton(xBoxControl, 1); + Button highRocket_c = new JoystickButton(xBoxControlArm, 3); + Button lowRocket_h = new JoystickButton(xBoxControlArm, 4); + Button cargoShip_h = new JoystickButton(xBoxControlArm, 6); + Button midRocket_h = new JoystickButton(xBoxControlArm, 5); public OI() { - // Button rTrig = new JoystickButton(xBoxControl, buttonNumber) - Button b1 = new JoystickButton(xBoxControl, 6); - b1.whenPressed(new SetSpeed(true)); - //b1.whenReleased(new SetSpeed(false)); - - Button b2 = new JoystickButton(xBoxControl, 5); - b2.whenPressed(new SetSpeed(false)); - b2.close(); - Button aButton = new JoystickButton(xBoxControl, 1); - aButton.whenPressed(new ArmToNextPosition()); - aButton.close(); + //Button rTrig = new JoystickButton(xBoxControl, buttonNumber) + pistonExtend.whenPressed(new ExtendPiston()); + + pistonRetract.whenPressed(new RetractPiston()); + climb.whenPressed(new ClimbStair()); + button.whenPressed(new TurnDegrees()); + + + lowRocket_c.whenPressed(new MoveArmTo(1)); + + midRocket_c.whenActive(new MoveArmTo(2)); + + highRocket_c.whenActive(new MoveArmTo(3)); + + + lowRocket_h.whenPressed(new MoveArmTo(4)); + + midRocket_h.whenPressed(new MoveArmTo(5)); + //not possible to do high rocket hatch due to no teleArm + + + cargoShip_h.whenPressed(new MoveArmTo(6)); + + /*Button aButton = new JoystickButton(xBoxControl, 1); + aButton.whenPressed(new ExtendPiston()); + aButton.whenReleased(new StopTeleArm());*/ } } diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index f45725d..db3dd1f 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -9,26 +9,26 @@ import edu.wpi.first.cameraserver.CameraServer; import edu.wpi.first.wpilibj.Compressor; -import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.TimedRobot; +import edu.wpi.first.wpilibj.Timer; import edu.wpi.first.wpilibj.command.Command; 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.commands.GetDistance; -import frc.robot.commands.PneumaticsDrive; import frc.robot.commands.Teleop; +import frc.robot.commands.TurnDegrees; import frc.robot.subsystems.Arm; -import frc.robot.subsystems.ArmLimitSwitch; -import frc.robot.subsystems.BaseLimitSwitch; import frc.robot.subsystems.CameraI2c; import frc.robot.subsystems.Claw; +import frc.robot.subsystems.Climb; import frc.robot.subsystems.DriveTrain; import frc.robot.subsystems.ExampleSubsystem; +import frc.robot.subsystems.LaserFinder; +import frc.robot.subsystems.Photoresistor; import frc.robot.subsystems.pneumatics; -import frc.robot.subsystems.SerialDistance; import frc.robot.subsystems.UltraSonic; +import frc.robot.subsystems.Wrist; /** * The VM is configured to automatically run this class, and to call the @@ -42,17 +42,21 @@ public class Robot extends TimedRobot { public static pneumatics m_pneumatics; public static OI m_oi; public static DriveTrain m_driveTrain; + //public static final ADIS16448_IMU imu = new ADIS16448_IMU(); public static Teleop m_teleop; Command m_autonomousCommand; Compressor com = new Compressor(0); SendableChooser m_chooser = new SendableChooser<>(); - public static SerialDistance m_serialPort; public static UltraSonic m_ultraSonic; public static Claw claw; public static CameraI2c camera; public static Arm arm; - public static ArmLimitSwitch m_limitSwitch; - public static BaseLimitSwitch m_baseSwitch; + public static Photoresistor resistor; + public static Timer m_timer; + public static LaserFinder m_laser; + public static Wrist m_wrist; + public static Climb m_climb; + //public static Ramps m_ramps; /** * This function is run when the robot is first started up and should be used * for any initialization code. @@ -60,12 +64,12 @@ public class Robot extends TimedRobot { */ @Override public void robotInit() { + SmartDashboard.putBoolean("Arm moving",false); com.setClosedLoopControl(true); com.start(); + m_climb = new Climb(); claw = new Claw(); arm = new Arm(); - m_baseSwitch = new BaseLimitSwitch(); - m_limitSwitch = new ArmLimitSwitch(); camera = new CameraI2c(); m_driveTrain = new DriveTrain(); m_chooser.setDefaultOption("Default Auto", new ExampleCommand()); @@ -74,11 +78,21 @@ public void robotInit() { CameraServer.getInstance().startAutomaticCapture(0); CameraServer.getInstance().startAutomaticCapture(1); Scheduler.getInstance().add(new Teleop()); - // m_serialPort = new SerialDistance(); - // m_ultraSonic = new UltraSonic(); - Scheduler.getInstance().add(new GetDistance()); + resistor = new Photoresistor(); + m_timer = new Timer(); + m_laser = new LaserFinder(); + SmartDashboard.putBoolean("TankDrive", false); + m_wrist = new Wrist(); + m_climb.dropFront(); + m_climb.dropBack(); + m_ultraSonic = new UltraSonic(); + + //Scheduler.getInstance().add(new GetDistance()); //OI must be init last + SmartDashboard.putNumber("Claw Speed", claw.speed); + SmartDashboard.putNumber("Offset", TurnDegrees.offset); m_oi = new OI(); + m_timer.start(); } /** @@ -91,7 +105,44 @@ public void robotInit() { */ @Override public void robotPeriodic() { + SmartDashboard.putNumber("Wrist Encoder", m_wrist.getAngle()); + SmartDashboard.putNumber("Reed switch", arm.readPos()); + SmartDashboard.putNumber("String pot", arm.readPos()); + SmartDashboard.putNumber("UltraSonic", m_ultraSonic.getDistance()); + //--------------------------Do 10 times per Second -------------------------------------------------- + if(m_timer.hasPeriodPassed(.1)){ + CameraI2c.read(); + m_laser.read(); + m_timer.reset(); + } + //////////////////////////////////////////////////////////////////////////////////////////////////// + //m_serialPort.getString(); //REMOVE LATER LSKJDFLKSDFLJAFHSDGJOEWJ9248&(*#@!^%(*!@&$(*@&$37))) + //CameraI2c.read(); + /* + SmartDashboard.putNumber("Gyro-X", m_driveTrain.getGyroX()); + SmartDashboard.putNumber("Gyro-Y", m_driveTrain.getGyroY()); + SmartDashboard.putN + umber("Gyro-Z", m_driveTrain.getGyroZ()); + */ + /* + TurnDegrees.offset = SmartDashboard.getNumber("Offset", TurnDegrees.offset); + SmartDashboard.putNumber("Offset", TurnDegrees.offset); + /* + SmartDashboard.putNumber("Gyro-X", m_driveTrain.getGyroX()); + SmartDashboard.putNumber("Gyro-Y", imu.getAngleY()); + SmartDashboard.putNumber("Gyro-Z", imu.getAngleZ()); + + SmartDashboard.putNumber("Accel-X", imu.getAccelX()); + SmartDashboard.putNumber("Accel-Y", imu.getAccelY()); + SmartDashboard.putNumber("Accel-Z", imu.getAccelZ()); + + SmartDashboard.putNumber("Pitch", imu.getPitch()); + SmartDashboard.putNumber("Roll", imu.getRoll()); + SmartDashboard.putNumber("Yaw", imu.getYaw()); + SmartDashboard.putNumber("Pressure: ", imu.getBarometricPressure()); + SmartDashboard.putNumber("Temperature: ", imu.getTemperature()); + */ } @Override public void disabledInit() { @@ -159,6 +210,7 @@ public void teleopInit() { */ @Override public void teleopPeriodic() { + //CameraI2c.read(); Scheduler.getInstance().run(); } diff --git a/src/main/java/frc/robot/RobotMap.java b/src/main/java/frc/robot/RobotMap.java index 8baa409..4694ef9 100644 --- a/src/main/java/frc/robot/RobotMap.java +++ b/src/main/java/frc/robot/RobotMap.java @@ -24,11 +24,42 @@ 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 int UltraSonic = 1; - public static int ArmLimitSwitch = 2; - public static int leftMotor1 = 0; - public static int leftMotor2 = 1; - public static int leftMotor3 = 2; - public static int leftMotor4 = 3; - public static int arm = 4; + + //Analog + public final static int PHOTORESISTOR = 0; + public final static int ULTRASONIC = 3; + public static final int STRING_POT = 2; + //public final static int ARMLIMITSWITCH = 2; + + //DIO + public final static int ARM_REED_2 = 2; + public final static int ARM_REED_3 = 3; + public final static int ARM_SWITCHBOTTOM = 0; + public final static int ARM_REED_1 = 1; //UNUSED(?) + //public final static int ULTRASONIC = 4; + public final static int ULTRASONIC_ECHO = 5; + public final static int WRIST_ENCODER_A = 6; + public final static int WRIST_ENCODER_B = 7; + + //CAN + public final static int ARM_L = 4;// + public final static int ARM_R = 5;// + public final static int CLAW_1 = 6;// + public final static int CLAW_2 = 8;// + public final static int LEFTMOTOR_1 = 3; // + public final static int LEFTMOTOR_2 = 1;// + public final static int RIGHTMOTOR_1 = 2;// + public final static int RIGHTMOTOR_2 = 0; // + public final static int WRISTMOTOR = 7;// + + //Pneumatics + public final static int PNEUMATIC_COMPRESSOR = 0; + public final static int DOUBLESOL_FORWARD = 0; + public final static int DOUBLESOL_REVERSE = 1; + public final static int DOUBLESOL_FORWARD1 = 2; + public static final int DOUBLESOL_REVERSE1 = 3; + public static final int CLIMBSOL_FRONT_FORWARD = 7; + public static final int CLIMBSOL_FRONT_REVERSE = 6; + public static final int CLIMBSOL_BACK_FORWARD = 4; + public static final int CLIMBSOL_BACK_REVERSE =5; } diff --git a/src/main/java/frc/robot/commands/ArmDrive.java b/src/main/java/frc/robot/commands/ArmDrive.java new file mode 100644 index 0000000..30bda95 --- /dev/null +++ b/src/main/java/frc/robot/commands/ArmDrive.java @@ -0,0 +1,73 @@ +/*----------------------------------------------------------------------------*/ +/* 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.GenericHID.Hand; +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.OI; +import frc.robot.Robot; + +public class ArmDrive extends Command { + public static final double WRIST_POSISTION_STRAIGHT = 15; + int count = 1; + public ArmDrive() { + // Use requires() here to declare subsystem dependencies + requires(Robot.arm); + requires(Robot.m_wrist); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + SmartDashboard.putBoolean("Swtich one", Robot.arm.switchOne()); + SmartDashboard.putBoolean("Switch two", Robot.arm.switchTwo()); + SmartDashboard.putBoolean("Switch three", Robot.arm.switchThree()); + SmartDashboard.putNumber("Arm position", Robot.arm.readPos()); + if(OI.xBoxControl.getBumper(Hand.kLeft)){ + Robot.arm.move(); + SmartDashboard.putBoolean("Arm moving", true); + }else if(OI.xBoxControl.getBumper(Hand.kRight)){ + Robot.arm.moveDown(); + SmartDashboard.putBoolean("Arm moving", true); + }else{ + SmartDashboard.putBoolean("Arm moving", false); + Robot.arm.stop(); + } + if(OI.xBoxControl.getY(Hand.kLeft) > .1 || OI.xBoxControl.getY(Hand.kLeft) < .1){ + Robot.m_wrist.raise(OI.xBoxControl.getY(Hand.kLeft)); + } + if(OI.xBoxControl.getStickButton(Hand.kLeft)){ + Robot.m_wrist.setTarget(WRIST_POSISTION_STRAIGHT); + Robot.m_wrist.move(); + } + + } + + // 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() { + } +} diff --git a/src/main/java/frc/robot/commands/ClawReverse.java b/src/main/java/frc/robot/commands/ClawDrive.java similarity index 81% rename from src/main/java/frc/robot/commands/ClawReverse.java rename to src/main/java/frc/robot/commands/ClawDrive.java index 775042c..274f54c 100644 --- a/src/main/java/frc/robot/commands/ClawReverse.java +++ b/src/main/java/frc/robot/commands/ClawDrive.java @@ -8,33 +8,29 @@ package frc.robot.commands; import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Robot; -import frc.robot.subsystems.Claw; -public class ClawReverse extends Command { - public ClawReverse() { +public class ClawDrive extends Command { + public ClawDrive() { // Use requires() here to declare subsystem dependencies - // eg. requires(chassis); requires(Robot.claw); } // Called just before this Command runs the first time @Override protected void initialize() { - SmartDashboard.putBoolean("Claw Reversed", true); - Claw.moveClaw(1); } // Called repeatedly when this Command is scheduled to run @Override protected void execute() { + Robot.claw.move(); } // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - return true; + return false; } // Called once after isFinished returns true diff --git a/src/main/java/frc/robot/commands/ClimbStair.java b/src/main/java/frc/robot/commands/ClimbStair.java new file mode 100644 index 0000000..c5ab40a --- /dev/null +++ b/src/main/java/frc/robot/commands/ClimbStair.java @@ -0,0 +1,82 @@ +/*----------------------------------------------------------------------------*/ +/* 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 ClimbStair extends Command { + boolean finished = false; + int step = 0; + // Step is 49 in deep + final static double firstDistance = 55; + final static double secondDistance = 29; + public ClimbStair() { + // Use requires() here to declare subsystem dependencies + requires(Robot.m_climb); + requires(Robot.m_driveTrain); + requires(Robot.m_ultraSonic); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + SmartDashboard.putBoolean("Climb run", true); + Robot.m_climb.liftFront(); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + if(step == 0){ + if(Robot.m_ultraSonic.getDistance() < firstDistance){ + step++; + }else{ + Robot.m_driveTrain.drive(.75, 0); + } + }else if(step == 1){ + Robot.m_climb.dropFront(); + step++; + }else if(step == 2){ + Robot.m_climb.liftBack(); + step++; + }else if(step == 3){ + if(Robot.m_ultraSonic.getDistance() > secondDistance){ + Robot.m_driveTrain.drive(.75, 0); + } + }else if(step == 4){ + Robot.m_climb.dropBack(); + }else if(step == 5){ + if(Robot.m_ultraSonic.getDistance() > 10){ + Robot.m_driveTrain.drive(.5, 0); + }else{ + step++; + } + }else if(step == 6){ + finished = true; + } + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return finished; + } + + // 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() { + } +} diff --git a/src/main/java/frc/robot/commands/ExtendPiston.java b/src/main/java/frc/robot/commands/ExtendPiston.java new file mode 100644 index 0000000..eba89c8 --- /dev/null +++ b/src/main/java/frc/robot/commands/ExtendPiston.java @@ -0,0 +1,46 @@ +/*----------------------------------------------------------------------------*/ +/* 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 frc.robot.Robot; + +public class ExtendPiston extends Command { + public ExtendPiston() { + // Use requires() here to declare subsystem dependencies + requires(Robot.m_climb); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + Robot.m_climb.liftFront(); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return true; + } + + // 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() { + } +} diff --git a/src/main/java/frc/robot/commands/GuideToTarget.java b/src/main/java/frc/robot/commands/GuideToTarget.java new file mode 100644 index 0000000..8c3327d --- /dev/null +++ b/src/main/java/frc/robot/commands/GuideToTarget.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 frc.robot.Robot; +import frc.robot.subsystems.CameraI2c; + +public class GuideToTarget extends Command { + public GuideToTarget() { + // Use requires() here to declare subsystem dependencies + requires(Robot.m_driveTrain); + requires(Robot.camera); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + + + } + + // 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() { + } +} diff --git a/src/main/java/frc/robot/commands/JoystickDrive.java b/src/main/java/frc/robot/commands/JoystickDrive.java index f8940ca..670e59a 100644 --- a/src/main/java/frc/robot/commands/JoystickDrive.java +++ b/src/main/java/frc/robot/commands/JoystickDrive.java @@ -9,11 +9,13 @@ import edu.wpi.first.wpilibj.GenericHID.Hand; import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.OI; import frc.robot.Robot; import frc.robot.subsystems.DriveTrain; public class JoystickDrive extends Command { + boolean tankDrive = false; public JoystickDrive() { // Use requires() here to declare subsystem dependencies // eg. requires(chassis); @@ -28,11 +30,23 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - //if(OI.xBoxControl.getBumperPressed(Hand.kRight)){ - DriveTrain.drive(OI.xBoxControl.getY(),OI.xBoxControl.getX()); - // }else{ - // DriveTrain.drive(OI.xBoxControl.getY(), OI.xBoxControl.getX()); - // } + double rightSpeed = 0.0; + double leftSpeed = 0.0; + + + /*if(tankDrive){ + leftSpeed = OI.xBoxControl.getY(Hand.kLeft); + rightSpeed = OI.xBoxControl.getY(Hand.kRight); + Robot.m_driveTrain.tankDrive(leftSpeed, rightSpeed); + }else{*/ + leftSpeed = -OI.xBoxControl.getX(Hand.kRight); + rightSpeed = -OI.xBoxControl.getY(Hand.kRight); + Robot.m_driveTrain.drive(rightSpeed, leftSpeed); + SmartDashboard.putNumber("Drive train value", rightSpeed); + //} + SmartDashboard.putNumber("GyroAngle", Robot.m_driveTrain.getGyroZ()); + //SmartDaShboard.putNumber("Left Speed: ", leftSpeed); + //SmartDashboard.putNumber("Right Speed: ", rightSpeed); } diff --git a/src/main/java/frc/robot/commands/MoveArmTo.java b/src/main/java/frc/robot/commands/MoveArmTo.java new file mode 100644 index 0000000..0eb5774 --- /dev/null +++ b/src/main/java/frc/robot/commands/MoveArmTo.java @@ -0,0 +1,94 @@ +/*----------------------------------------------------------------------------*/ +/* 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; +import frc.robot.subsystems.Arm; +import frc.robot.subsystems.Wrist; + +public class MoveArmTo extends Command { + int position = 0; + int target; + boolean isDown; + boolean finished; + int currentPos; + double WRIST_ANGLE_TOP = 15; + double WRIST_ANGLE_BOTTOM = 1; + + public MoveArmTo(int target) { + // Use requires() here to declare subsystem dependencies + requires(Robot.arm); + requires(Robot.m_wrist); + this.target = target; + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + position = Robot.arm.readPos(); + if(Robot.arm.readPos() < target){ + isDown = false; + }else if(Robot.arm.readPos() > target){ + isDown = true; + } + finished = false; + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + if(Robot.arm.readPos() == 1 && target == 1){ + finished = true; + }else if(Robot.arm.readPos() == 2 && target == 2){ + finished = true; + }else if(Robot.arm.readPos() == 3 && target == 3){ + finished = true; + }else{ + finished = false; + } + if(target == 7){ + Robot.m_wrist.setTarget(WRIST_ANGLE_TOP); + } + if(target != 7){ + Robot.m_wrist.setTarget(WRIST_ANGLE_BOTTOM); + } + if(!isDown){ + Robot.m_wrist.move(); + }else{ + Robot.arm.moveDown(); + } + + SmartDashboard.putBoolean("Arm Done", finished); + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return finished && Robot.m_wrist.getFinished(); + } + + // Called once after isFinished returns true + @Override + protected void end() { + Robot.arm.stop(); + Robot.m_wrist.stop(); + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + @Override + protected void interrupted() { + } + + public void setTarget(int target) { + this.target = target; + } +} diff --git a/src/main/java/frc/robot/commands/PneumaticsDrive.java b/src/main/java/frc/robot/commands/PneumaticsDrive.java index b3c3dad..679ef87 100644 --- a/src/main/java/frc/robot/commands/PneumaticsDrive.java +++ b/src/main/java/frc/robot/commands/PneumaticsDrive.java @@ -6,12 +6,9 @@ /*----------------------------------------------------------------------------*/ package frc.robot.commands; - -import edu.wpi.first.wpilibj.GenericHID.Hand; import edu.wpi.first.wpilibj.command.Command; import frc.robot.OI; import frc.robot.Robot; -import frc.robot.subsystems.pneumatics; public class PneumaticsDrive extends Command { public PneumaticsDrive() { @@ -22,18 +19,28 @@ public PneumaticsDrive() { // Called just before this Command runs the first time @Override protected void initialize() { - System.out.println("init"); + //System.out.println("init"); } // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - if(OI.xBoxControl.getTriggerAxis(Hand.kLeft)>0) { + /*if(OI.xBoxControl.getBumperPressed(Hand.kLeft)) { pneumatics.retract(); } - if(OI.xBoxControl.getTriggerAxis(Hand.kRight)>0) { + if(OI.xBoxControl.getBumperPressed(Hand.kRight)) { pneumatics.extend(); } + */ + if(OI.xBoxControl.getYButtonPressed()){ + Robot.m_pneumatics.toggleOut(); + if(Robot.m_pneumatics.getOut()){ + Robot.m_pneumatics.retract(); + }else{ + Robot.m_pneumatics.extend(); + } + } + } diff --git a/src/main/java/frc/robot/commands/RetractPiston.java b/src/main/java/frc/robot/commands/RetractPiston.java new file mode 100644 index 0000000..5a99a1c --- /dev/null +++ b/src/main/java/frc/robot/commands/RetractPiston.java @@ -0,0 +1,46 @@ +/*----------------------------------------------------------------------------*/ +/* 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 frc.robot.Robot; + +public class RetractPiston extends Command { + public RetractPiston() { + // Use requires() here to declare subsystem dependencies + requires(Robot.m_climb); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + Robot.m_climb.dropFront(); + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + } + + // Make this return true when this Command no longer needs to run execute() + @Override + protected boolean isFinished() { + return true; + } + + // 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() { + } +} diff --git a/src/main/java/frc/robot/commands/Rumbler.java b/src/main/java/frc/robot/commands/Rumbler.java new file mode 100644 index 0000000..f0945b1 --- /dev/null +++ b/src/main/java/frc/robot/commands/Rumbler.java @@ -0,0 +1,75 @@ +/*----------------------------------------------------------------------------*/ +/* 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.GenericHID.RumbleType; +import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.OI; +import frc.robot.Robot; +import frc.robot.subsystems.UltraSonic; + +public class Rumbler extends Command { +public static double zone = 18; +public static boolean warn = false; +int count = 0; + public Rumbler() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + requires(Robot.m_ultraSonic); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + double distance = Robot.m_ultraSonic.getDistance(); + if(count >= 5){ + if(distance < zone){ + OI.xBoxControl.setRumble(RumbleType.kLeftRumble, 1 - (distance/zone)); + OI.xBoxControl.setRumble(RumbleType.kRightRumble, 1 - (distance/zone)); + }else{ + OI.xBoxControl.setRumble(RumbleType.kLeftRumble, 0); + OI.xBoxControl.setRumble(RumbleType.kRightRumble, 0); + } + if(distance < zone) { + warn = true; + } + else { + warn = false; + } + + SmartDashboard.putNumber("Ultrasonic Distance", distance); + SmartDashboard.putBoolean("WARNING", warn); + count = 0; + } + count++; + + } + + // 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() { + } +} diff --git a/src/main/java/frc/robot/commands/ArmToNextPosition.java b/src/main/java/frc/robot/commands/StopArm.java similarity index 65% rename from src/main/java/frc/robot/commands/ArmToNextPosition.java rename to src/main/java/frc/robot/commands/StopArm.java index f141cef..9582c83 100644 --- a/src/main/java/frc/robot/commands/ArmToNextPosition.java +++ b/src/main/java/frc/robot/commands/StopArm.java @@ -9,49 +9,28 @@ import edu.wpi.first.wpilibj.command.Command; import frc.robot.Robot; -import frc.robot.subsystems.Arm; -import frc.robot.subsystems.ArmLimitSwitch; -import frc.robot.subsystems.BaseLimitSwitch; -public class ArmToNextPosition extends Command { - boolean startRaised = false; - static final double speed = 1; - public ArmToNextPosition() { +public class StopArm extends Command { + public StopArm() { // Use requires() here to declare subsystem dependencies requires(Robot.arm); - requires(Robot.m_baseSwitch); - requires(Robot.m_limitSwitch); } // Called just before this Command runs the first time @Override protected void initialize() { - startRaised = ArmLimitSwitch.getState(); + Robot.arm.stop(); } // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - if(startRaised){ - Arm.raise(speed); - }else if(!startRaised){ - Arm.raise(-speed); - } } // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - if(startRaised){ - if(BaseLimitSwitch.getState()){ - return true; - } - }else{ - if(ArmLimitSwitch.getState()){ - return true; - } - } - return false; + return true; } // Called once after isFinished returns true diff --git a/src/main/java/frc/robot/commands/StopRamps.java b/src/main/java/frc/robot/commands/StopRamps.java new file mode 100644 index 0000000..8ff06e0 --- /dev/null +++ b/src/main/java/frc/robot/commands/StopRamps.java @@ -0,0 +1,45 @@ +/*----------------------------------------------------------------------------*/ +/* 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; + +public class StopRamps extends Command { + public StopRamps() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + //Robot.m_ramps.stop + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + } + + // 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() { + } +} diff --git a/src/main/java/frc/robot/commands/TurnDegrees.java b/src/main/java/frc/robot/commands/TurnDegrees.java index ffebab5..c9f2dfb 100644 --- a/src/main/java/frc/robot/commands/TurnDegrees.java +++ b/src/main/java/frc/robot/commands/TurnDegrees.java @@ -8,32 +8,91 @@ package frc.robot.commands; import edu.wpi.first.wpilibj.command.Command; +import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Robot; +import frc.robot.subsystems.CameraI2c; +import frc.robot.subsystems.DriveTrain; public class TurnDegrees extends Command { - int degrees = 0; - public TurnDegrees(int degrees) { + boolean finished = false; + public static double offset = 6; + public double target; + public double turnAngle; + public boolean isLeft; + double leftSpeed = 0; + double rightSpeed = 0; + public TurnDegrees() { // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); requires(Robot.m_driveTrain); - this.degrees = degrees; + } // Called just before this Command runs the first time @Override protected void initialize() { - + this.target = CameraI2c.read(); + //SmartDashboard.putNumber("Target", target); + isLeft = target<0; + //SmartDashboard.putBoolean("IsLeft", isLeft); + if(isLeft){ + turnAngle = Robot.m_driveTrain.getGyroZ() - Math.abs(target); + }else{ + turnAngle = Robot.m_driveTrain.getGyroZ() + Math.abs(target); + } + //SmartDashboard.putNumber("Gyro angle of target", turnAngle); + //SmartDashboard.putNumber("Angle to target", target); + + if(isLeft) { + leftSpeed = -.5; + rightSpeed = .5; + } + else { + leftSpeed = .5; + rightSpeed = -.5; + } + + + } // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - + Robot.m_driveTrain.tankDrive(leftSpeed, rightSpeed); + SmartDashboard.putNumber("Offset", offset); + SmartDashboard.getNumber("Offset", offset); } // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - return false; + Double gyroZ = Robot.m_driveTrain.getGyroZ(); + if(target == 0) + finished = true; + else { + if(isLeft) { + if(gyroZ <= turnAngle + offset){ + finished = true; + }else{ + finished = false; + } + } + else { + if(gyroZ >= turnAngle - offset){ + + finished = true; + }else{ + finished = false; + } + } + } + //SmartDashboard.putNumber("GyroZ turnangle", gyroZ); + //SmartDashboard.putNumber("TurnAngle",turnAngle); + //SmartDashboard.putBoolean("Stopped", finished); + + return finished; } // Called once after isFinished returns true diff --git a/src/main/java/frc/robot/commands/GetDistance.java b/src/main/java/frc/robot/commands/TurnWrist.java similarity index 72% rename from src/main/java/frc/robot/commands/GetDistance.java rename to src/main/java/frc/robot/commands/TurnWrist.java index 8484aeb..3e3c024 100644 --- a/src/main/java/frc/robot/commands/GetDistance.java +++ b/src/main/java/frc/robot/commands/TurnWrist.java @@ -8,38 +8,38 @@ package frc.robot.commands; import edu.wpi.first.wpilibj.command.Command; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Robot; -import frc.robot.subsystems.SerialDistance; -public class GetDistance extends Command { - - String distance; - Double distanceDouble; - - public GetDistance() { +public class TurnWrist extends Command { + private double targetAngle; + public TurnWrist(double targetAngle) { // Use requires() here to declare subsystem dependencies // eg. requires(chassis); + requires(Robot.m_wrist); + this.targetAngle = targetAngle; } // Called just before this Command runs the first time @Override protected void initialize() { + Robot.m_wrist.setTarget(targetAngle); } // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - distance = Robot.m_serialPort.getString(); - SmartDashboard.putString("distance", distance.replace("R","")); - distanceDouble = Robot.m_ultraSonic.distance(); - SmartDashboard.putString("Distance double", distanceDouble +""); + Robot.m_wrist.move(); } // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - return false; + if(Robot.m_wrist.getAngle() == targetAngle) { + return true; + } + else { + return false; + } } // Called once after isFinished returns true diff --git a/src/main/java/frc/robot/commands/MoveClaw.java b/src/main/java/frc/robot/commands/getPhotoresistorValue.java similarity index 85% rename from src/main/java/frc/robot/commands/MoveClaw.java rename to src/main/java/frc/robot/commands/getPhotoresistorValue.java index 46ec6bc..701760e 100644 --- a/src/main/java/frc/robot/commands/MoveClaw.java +++ b/src/main/java/frc/robot/commands/getPhotoresistorValue.java @@ -10,30 +10,28 @@ import edu.wpi.first.wpilibj.command.Command; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import frc.robot.Robot; -import frc.robot.subsystems.Claw; -public class MoveClaw extends Command { - public MoveClaw() { +public class getPhotoresistorValue extends Command { + public getPhotoresistorValue() { // Use requires() here to declare subsystem dependencies - requires(Robot.claw); + requires(Robot.resistor); } // Called just before this Command runs the first time @Override protected void initialize() { - Claw.moveClaw(-1); - SmartDashboard.putBoolean("Claw Forward", true); } // Called repeatedly when this Command is scheduled to run @Override protected void execute() { + SmartDashboard.putNumber("PhotoResistor value", Robot.resistor.value()); } // Make this return true when this Command no longer needs to run execute() @Override protected boolean isFinished() { - return true; + return false; } // Called once after isFinished returns true diff --git a/src/main/java/frc/robot/commands/laserUpdater.java b/src/main/java/frc/robot/commands/laserUpdater.java new file mode 100644 index 0000000..16d074e --- /dev/null +++ b/src/main/java/frc/robot/commands/laserUpdater.java @@ -0,0 +1,46 @@ +/*----------------------------------------------------------------------------*/ +/* 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 frc.robot.Robot; + +public class laserUpdater extends Command { + public laserUpdater() { + // Use requires() here to declare subsystem dependencies + requires(Robot.m_laser); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() { + } + + // Called repeatedly when this Command is scheduled to run + @Override + protected void execute() { + Robot.m_laser.read(); + } + + // 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() { + } +} diff --git a/src/main/java/frc/robot/commands/updateAngleToTarget.java b/src/main/java/frc/robot/commands/updateAngleToTarget.java index ead963e..f02ba2f 100644 --- a/src/main/java/frc/robot/commands/updateAngleToTarget.java +++ b/src/main/java/frc/robot/commands/updateAngleToTarget.java @@ -27,7 +27,7 @@ protected void initialize() { // Called repeatedly when this Command is scheduled to run @Override protected void execute() { - CameraI2c.updateAngle(); + CameraI2c.read(); } // Make this return true when this Command no longer needs to run execute() diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 9e00504..258a720 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -7,26 +7,76 @@ package frc.robot.subsystems; -import edu.wpi.first.wpilibj.Spark; + +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.command.Subsystem; -import frc.robot.Robot; import frc.robot.RobotMap; -import frc.robot.commands.ArmToNextPosition; +import frc.robot.commands.ArmDrive; /** * Add your docs here. */ public class Arm extends Subsystem { - static Spark motor = new Spark(RobotMap.arm); + //REMEMBER TO REPLACE WITH ACTUAL MEASUREMENTS))(*@&*&%(*($*!@&$@!*&#^*&@!$ + public int position = 0; + public static int MARGIN_OF_ERROR = 30; + //WPI_TalonSRX motor; + private static final double leftMod = .88; + private static final double rightMod = 1; + private static WPI_TalonSRX motorL = new WPI_TalonSRX(RobotMap.ARM_L); + private static WPI_TalonSRX motorR = new WPI_TalonSRX(RobotMap.ARM_R); + double speed = 1; + DigitalInput switchBottom = new DigitalInput(RobotMap.ARM_SWITCHBOTTOM); + DigitalInput reedSwitch_1 = new DigitalInput(RobotMap.ARM_REED_1); + DigitalInput reedSwitch_2 = new DigitalInput(RobotMap.ARM_REED_2); + DigitalInput reedSwitch_3 = new DigitalInput(RobotMap.ARM_REED_3); // Put methods for controlling this subsystem // here. Call these from Commands. - + public Arm() { + //motor = new WPI_TalonSRX(3); + } @Override public void initDefaultCommand() { + //setDefaultCommand(new MoveArmTo(7)); + setDefaultCommand(new ArmDrive()); } - public static void raise(Double speed){ - if(ArmLimitSwitch.getState()){ - motor.set(speed); + + public void move(){ + + motorL.set(speed *leftMod); + motorR.set(speed * rightMod); + } + + public int readPos(){ + if(reedSwitch_1.get()){ + position = 1; + }else if(reedSwitch_2.get()){ + position = 2; + }else if(reedSwitch_3.get()){ + position = 3; } + return position; + } + + public boolean getSwitch(){ + return switchBottom.get(); + } + public void moveDown(){ + motorL.set(-speed *leftMod); + motorR.set(-speed * rightMod); + } + public void stop(){ + motorL.set(0); + motorR.set(0); + } + public boolean switchOne(){ + return reedSwitch_1.get(); + } + public boolean switchTwo(){ + return reedSwitch_2.get(); + } + public boolean switchThree(){ + return reedSwitch_3.get(); } -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/CameraI2c.java b/src/main/java/frc/robot/subsystems/CameraI2c.java index bb45150..62d043b 100644 --- a/src/main/java/frc/robot/subsystems/CameraI2c.java +++ b/src/main/java/frc/robot/subsystems/CameraI2c.java @@ -25,13 +25,10 @@ public class CameraI2c extends Subsystem { @Override public void initDefaultCommand() { // Set the default command for a subsystem here. - setDefaultCommand(new updateAngleToTarget()); + // setDefaultCommand(new updateAngleToTarget()); } - public static void updateAngle(){ - if(true){ - read(); - SmartDashboard.putNumber("Angle to target", angle); - } + public static int getAngle(){ + return angle; } public static int read(){ String received = ""; @@ -48,6 +45,8 @@ public static int read(){ if(!received.isBlank()){ angle = Integer.parseInt(received); } + SmartDashboard.putString("RAW from arduino", received); + SmartDashboard.putNumber("Angle to target", angle); return angle; } } diff --git a/src/main/java/frc/robot/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Claw.java index 8f0e687..d844092 100644 --- a/src/main/java/frc/robot/subsystems/Claw.java +++ b/src/main/java/frc/robot/subsystems/Claw.java @@ -7,30 +7,48 @@ package frc.robot.subsystems; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; + import edu.wpi.first.wpilibj.Spark; import edu.wpi.first.wpilibj.SpeedControllerGroup; +import edu.wpi.first.wpilibj.GenericHID.Hand; import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import frc.robot.commands.MoveClaw; - +import frc.robot.OI; +import frc.robot.RobotMap; +import frc.robot.commands.ClawDrive; /** * Add your docs here. */ public class Claw extends Subsystem { // Put methods for controlling this subsystem // here. Call these from Commands. - static double speedModifier = 1.0; + public double speed = 0; @Override public void initDefaultCommand() { // Set the default command for a subsystem here. - setDefaultCommand(new MoveClaw()); + setDefaultCommand(new ClawDrive()); + } + static WPI_TalonSRX claw1 = new WPI_TalonSRX(RobotMap.CLAW_1); + static WPI_TalonSRX claw2 = new WPI_TalonSRX(RobotMap.CLAW_2); + public void moveForward(){ + speed = SmartDashboard.getNumber("Claw Speed", speed); + claw2.set(speed); + claw1.set(speed); + } + public void moveBackward(){ + speed = SmartDashboard.getNumber("Claw Speed", speed); + claw2.set(-speed); + claw1.set(-speed); + } + public void stop(){ + claw1.set(0); + claw2.set(0); } - static Spark claw1 = new Spark(4); - static Spark claw2 = new Spark(5); - public static void moveClaw(double speed){ - claw1.set(speed * speedModifier); - claw2.set(speed * speedModifier); - SmartDashboard.putNumber("Claw Speed", speed); - + public void move(){ + double speed = OI.xBoxControl.getTriggerAxis(Hand.kLeft); + double backSpeed = OI.xBoxControl.getTriggerAxis(Hand.kRight); + claw1.set(speed - backSpeed); + claw2.set(speed - backSpeed); } } diff --git a/src/main/java/frc/robot/subsystems/BaseLimitSwitch.java b/src/main/java/frc/robot/subsystems/Climb.java similarity index 51% rename from src/main/java/frc/robot/subsystems/BaseLimitSwitch.java rename to src/main/java/frc/robot/subsystems/Climb.java index 26ef15b..dd2ca5b 100644 --- a/src/main/java/frc/robot/subsystems/BaseLimitSwitch.java +++ b/src/main/java/frc/robot/subsystems/Climb.java @@ -7,25 +7,34 @@ package frc.robot.subsystems; -import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.DoubleSolenoid; import edu.wpi.first.wpilibj.command.Subsystem; +import frc.robot.RobotMap; /** * Add your docs here. */ -public class BaseLimitSwitch extends Subsystem { +public class Climb extends Subsystem { // Put methods for controlling this subsystem - public static boolean state; - static DigitalInput Switch = new DigitalInput(4); - - + // here. Call these from Commands. + DoubleSolenoid frontPiston = new DoubleSolenoid(RobotMap.CLIMBSOL_FRONT_FORWARD, RobotMap.CLIMBSOL_FRONT_REVERSE); + DoubleSolenoid backPiston = new DoubleSolenoid(RobotMap.CLIMBSOL_BACK_FORWARD, RobotMap.CLIMBSOL_BACK_REVERSE); @Override public void initDefaultCommand() { // Set the default command for a subsystem here. // setDefaultCommand(new MySpecialCommand()); } - public static boolean getState(){ - return Switch.get(); + public void liftFront(){ + frontPiston.set(DoubleSolenoid.Value.kForward); + backPiston.set(DoubleSolenoid.Value.kReverse); + } + public void dropFront(){ + frontPiston.set(DoubleSolenoid.Value.kReverse); + } + public void liftBack(){ + backPiston.set(DoubleSolenoid.Value.kForward); + } + public void dropBack(){ + backPiston.set(DoubleSolenoid.Value.kReverse); } - } diff --git a/src/main/java/frc/robot/subsystems/DriveTrain.java b/src/main/java/frc/robot/subsystems/DriveTrain.java index a62b13c..b8bc7ed 100644 --- a/src/main/java/frc/robot/subsystems/DriveTrain.java +++ b/src/main/java/frc/robot/subsystems/DriveTrain.java @@ -13,7 +13,12 @@ import edu.wpi.first.wpilibj.drive.DifferentialDrive; import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.RobotMap; +import frc.robot.commands.GuideToTarget; import frc.robot.commands.JoystickDrive; +import com.analog.adis16448.frc.*; + +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; /** * Add your docs here. @@ -21,38 +26,60 @@ public class DriveTrain extends Subsystem { // Put methods for controlling this subsystem // here. Call these from Commands. - public static boolean fast; - private static Double speedModifier = 1.0; - public Spark sparkL1 = new Spark(0); - public Spark sparkL2 = new Spark(1); - public Spark sparkR1 = new Spark(2); - public Spark sparkR2 = new Spark(3); + + public WPI_TalonSRX talonL1; + public WPI_TalonSRX talonL2; + public WPI_TalonSRX talonR1; + public WPI_TalonSRX talonR2; + + public static final ADIS16448_IMU imu = new ADIS16448_IMU(); + public static Double speedModifier; + private final double exponentialGrowth = .5; + + //public Spark sparkL1; + //public Spark sparkL2; + //public Spark sparkR1; + //public Spark sparkR2; + SpeedControllerGroup leftGroup; SpeedControllerGroup rightGroup; - public static DifferentialDrive diffDrive; + public DifferentialDrive diffDrive; public DriveTrain() { - leftGroup = new SpeedControllerGroup(sparkL1, sparkL2); - rightGroup = new SpeedControllerGroup(sparkR1, sparkR2); + //sparkL1 = new Spark(RobotMap.LEFTMOTOR_1); + //sparkL2 = new Spark(RobotMap.LEFTMOTOR_2); + //sparkR1 = new Spark(RobotMap.RIGHTMOTOR_1); + //sparkR2 = new Spark(RobotMap.RIGHTMOTOR_2); + talonL1 = new WPI_TalonSRX(RobotMap.LEFTMOTOR_1); + talonL2 = new WPI_TalonSRX(RobotMap.LEFTMOTOR_2); + talonR1 = new WPI_TalonSRX(RobotMap.RIGHTMOTOR_1); + talonR2 = new WPI_TalonSRX(RobotMap.RIGHTMOTOR_2); - diffDrive = new DifferentialDrive(leftGroup, rightGroup); + speedModifier = 1.0; + + leftGroup = new SpeedControllerGroup(talonL1, talonL2); + rightGroup = new SpeedControllerGroup(talonR1, talonR2); + diffDrive = new DifferentialDrive(leftGroup, rightGroup); + imu.reset(); + imu.calibrate(); } - public static void drive(double leftSpeed, double rightSpeed) { - diffDrive.arcadeDrive(leftSpeed * speedModifier, rightSpeed * speedModifier); - Shuffleboard.selectTab("Live Window"); - Shuffleboard.update(); - SmartDashboard.putNumber("SpeedModifier", speedModifier); + //maybe change back to static (broken code?) + public void drive(double leftSpeed, double rightSpeed) { + diffDrive.arcadeDrive(-leftSpeed * speedModifier, -rightSpeed * speedModifier); + } + public void tankDrive(double leftSpeed, double rightSpeed){ + diffDrive.tankDrive(-(Math.pow(rightSpeed - 1, 3) + .5*(rightSpeed-1) + 1.5) * .75, -(Math.pow(leftSpeed - 1, 3) + .5*(leftSpeed - 1) + 1.5) * .75); } @Override - public void initDefaultCommand() { + public void initDefaultCommand() { // Set the default command for a subsystem here. setDefaultCommand(new JoystickDrive()); } @@ -60,6 +87,10 @@ public void setFast(){ speedModifier = 1.0; } public void setSlow(){ - speedModifier = 0.5; + speedModifier = 0.65; } + public double getGyroZ(){ + return imu.getAngleZ(); + } + } diff --git a/src/main/java/frc/robot/subsystems/LaserFinder.java b/src/main/java/frc/robot/subsystems/LaserFinder.java new file mode 100644 index 0000000..e3de695 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/LaserFinder.java @@ -0,0 +1,53 @@ +/*----------------------------------------------------------------------------*/ +/* 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.I2C; +import edu.wpi.first.wpilibj.I2C.Port; +import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.commands.laserUpdater; + +/** + * Add your docs here. + */ +public class LaserFinder extends Subsystem { + // Put methods for controlling this subsystem + // here. Call these from Commands. + public I2C Wire; + public double distance; + public LaserFinder(){ + Wire = new I2C(Port.kOnboard, 8); + } + @Override + public void initDefaultCommand() { + // Set the default command for a subsystem here. + setDefaultCommand(new laserUpdater()); + } + + public double read(){ + String received = "Error"; + char[] ch = new char[16]; + byte[] toSend = new byte[1]; + byte[] fromArduino = new byte[16]; + Wire.transaction(toSend, 1, fromArduino, 16); + for(int i = 0; i < fromArduino.length; i++){ + ch[i] = (char) fromArduino[i]; + if(ch[i] == '-' || ch[i] == '0' || ch[i] == '1' || ch[i] == '2' || ch[i] == '3'|| ch[i] == '4' || ch[i] == '5' || ch[i] == '6' || ch[i] == '7'|| ch[i] == '8' || ch[i] == '9'){ + received += ch[i]; + }else{received += ch[i];} + + } + if(!received.isBlank()){ + //distance = Double.parseDouble(received); + } + SmartDashboard.putString("Distance from laser", received); + return distance; + } + +} diff --git a/src/main/java/frc/robot/subsystems/ArmLimitSwitch.java b/src/main/java/frc/robot/subsystems/Photoresistor.java similarity index 70% rename from src/main/java/frc/robot/subsystems/ArmLimitSwitch.java rename to src/main/java/frc/robot/subsystems/Photoresistor.java index ceff811..86a6391 100644 --- a/src/main/java/frc/robot/subsystems/ArmLimitSwitch.java +++ b/src/main/java/frc/robot/subsystems/Photoresistor.java @@ -7,24 +7,25 @@ package frc.robot.subsystems; -import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.AnalogInput; import edu.wpi.first.wpilibj.command.Subsystem; +import frc.robot.RobotMap; +import frc.robot.commands.getPhotoresistorValue; /** * Add your docs here. */ -public class ArmLimitSwitch extends Subsystem { - public static boolean state; - static DigitalInput Switch = new DigitalInput(3); +public class Photoresistor extends Subsystem { + AnalogInput sensor = new AnalogInput(RobotMap.PHOTORESISTOR); // 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()); + setDefaultCommand(new getPhotoresistorValue()); } - public static boolean getState(){ - return Switch.get(); + public double value(){ + return sensor.getVoltage(); } } diff --git a/src/main/java/frc/robot/subsystems/SerialDistance.java b/src/main/java/frc/robot/subsystems/SerialDistance.java deleted file mode 100644 index 0c2443a..0000000 --- a/src/main/java/frc/robot/subsystems/SerialDistance.java +++ /dev/null @@ -1,34 +0,0 @@ -/*----------------------------------------------------------------------------*/ -/* 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.Subsystem; -import frc.robot.commands.GetDistance; -import edu.wpi.first.wpilibj.AnalogInput; -import edu.wpi.first.wpilibj.SerialPort; - -/** - * Add your docs here. - */ -public class SerialDistance extends Subsystem { - // Put methods for controlling this subsystem - // here. Call these from Commands. - - //SerialPort sensor = new SerialPort(9600, SerialPort.Port.kOnboard, 8, SerialPort.Parity.kNone, SerialPort.StopBits.kOne); - @Override - public void initDefaultCommand() { - // Set the default command for a subsystem here. - setDefaultCommand(new GetDistance()); - } - - public String getString() - { - // return sensor.readString(); - return ""; - } -} diff --git a/src/main/java/frc/robot/subsystems/UltraSonic.java b/src/main/java/frc/robot/subsystems/UltraSonic.java index 327b291..697390b 100644 --- a/src/main/java/frc/robot/subsystems/UltraSonic.java +++ b/src/main/java/frc/robot/subsystems/UltraSonic.java @@ -8,8 +8,12 @@ package frc.robot.subsystems; import edu.wpi.first.wpilibj.AnalogInput; +import edu.wpi.first.wpilibj.Ultrasonic; +import edu.wpi.first.wpilibj.Ultrasonic.Unit; import edu.wpi.first.wpilibj.command.Subsystem; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import frc.robot.RobotMap; +import frc.robot.commands.Rumbler; /** * Add your docs here. @@ -17,23 +21,18 @@ public class UltraSonic extends Subsystem { // Put methods for controlling this subsystem // here. Call these from Commands. - - AnalogInput sonicSensor = new AnalogInput(0); + AnalogInput in = new AnalogInput(RobotMap.ULTRASONIC); + //Ultrasonic us = new Ultrasonic (RobotMap.ULTRASONIC, RobotMap.ULTRASONIC_ECHO, Unit.kInches); + + public double getDistance(){ + return in.getVoltage() * 1024 / 25.4; + //return us.getRangeInches(); + } @Override public void initDefaultCommand() { // Set the default command for a subsystem here. // setDefaultCommand(new MySpecialCommand()); - - } - public Double distance(){ - return getVoltage()* 100/512;//("volt" * scale factor/sensitivity) - } - - public Double getVoltage() - { - Double volt = sonicSensor.getVoltage(); - SmartDashboard.putNumber("AnalogInput Voltage", volt); - return volt; + setDefaultCommand(new Rumbler()); } } diff --git a/src/main/java/frc/robot/subsystems/Wrist.java b/src/main/java/frc/robot/subsystems/Wrist.java new file mode 100644 index 0000000..a7cd7e2 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Wrist.java @@ -0,0 +1,99 @@ +/*----------------------------------------------------------------------------*/ +/* 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.DigitalInput; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.CounterBase.EncodingType; +import edu.wpi.first.wpilibj.command.Subsystem; +import frc.robot.RobotMap; + +/** + * Add your docs here. + */ +public class Wrist extends Subsystem { + // Put methods for controlling this subsystem + // here. Call these from Commands. + private final static double SPEED = .85; + //num of angles = 1 rotation is it 360? no one knows + private final static double ANGLE_TO_ROTATION = 360; + public static final double TOP = 100; + public static final double BOTTOM = 0; + private boolean finished = false; + private double targetAngle; + double speed1, speed2, speed3; + Encoder wristEncoder; + DigitalInput switchBottom; + DigitalInput switchTop; + + WPI_TalonSRX wristTalon; + + @Override + public void initDefaultCommand() { + // Set the default command for a subsystem here. + // setDefaultCommand(new MySpecialCommand()); + } + + public Wrist() { + wristEncoder = new Encoder(RobotMap.WRIST_ENCODER_A, RobotMap.WRIST_ENCODER_B, false, EncodingType.k4X); + wristTalon = new WPI_TalonSRX(RobotMap.WRISTMOTOR); + } + + public void move() { + finished = false; + boolean down = getAngle()>targetAngle; + //forward = down backwards = up + if(down){ + if(getAngle() < targetAngle || getAngle() <= BOTTOM){ + finished = true; + wristTalon.set(0); + }else{ + finished = false; + wristTalon.set(SPEED); + } + }else{ + if(getAngle() > targetAngle || getAngle() >= TOP) { + finished = true; + wristTalon.set(0); + } + else { + wristTalon.set(-SPEED); + } + + } + + } + + public boolean atTarget() { + return getAngle() == targetAngle; + } + public void setTarget(double target) { + targetAngle = target; + } + public boolean getFinished(){ + return finished; + } + public double getAngle(){ + return wristEncoder.getDistance(); + } + public void stop(){ + wristTalon.set(0); + } + public void raise(double speed){ + speed = -speed; + speed3 = speed2; + speed2 = speed1; + speed1 = speed; + wristTalon.set(speed * SPEED); + } + public void lower(){ + wristTalon.set(SPEED); + } +} diff --git a/src/main/java/frc/robot/subsystems/pneumatics.java b/src/main/java/frc/robot/subsystems/pneumatics.java index bc285de..5ad6be2 100644 --- a/src/main/java/frc/robot/subsystems/pneumatics.java +++ b/src/main/java/frc/robot/subsystems/pneumatics.java @@ -9,20 +9,26 @@ import edu.wpi.first.wpilibj.Compressor; import edu.wpi.first.wpilibj.DoubleSolenoid; +import edu.wpi.first.wpilibj.Solenoid; import edu.wpi.first.wpilibj.command.Subsystem; +import frc.robot.RobotMap; import frc.robot.commands.PneumaticsDrive; /** * Add your docs here. */ public class pneumatics extends Subsystem { + boolean out = false; // Put methods for controlling this subsystem // here. Call these from Commands. - public static Compressor compressor = new Compressor(0); - public static DoubleSolenoid doubleSol = new DoubleSolenoid(0, 1); - public pneumatics(){ - System.out.println("Compressor Init"); - compressor = new Compressor(0); + public static Compressor compressor; + //public static Solenoid forward = new Solenoid(RobotMap.DOUBLESOL_FORWARD); + ///public static Solenoid backwards = new Solenoid(RobotMap.DOUBLESOL_REVERSE); + + public static DoubleSolenoid doubleSol = new DoubleSolenoid(RobotMap.DOUBLESOL_FORWARD, RobotMap.DOUBLESOL_REVERSE); + public static DoubleSolenoid doubleSol2 = new DoubleSolenoid(RobotMap.DOUBLESOL_FORWARD1, RobotMap.DOUBLESOL_REVERSE1); + public pneumatics() { + compressor = new Compressor(RobotMap.PNEUMATIC_COMPRESSOR); compressor.setClosedLoopControl(true); compressor.start(); } @@ -33,13 +39,26 @@ public void initDefaultCommand() { setDefaultCommand(new PneumaticsDrive()); } - public static void extend() { + public void extend() { + //backwards.set(false); + //forward.set(true); doubleSol.set(DoubleSolenoid.Value.kForward); + doubleSol2.set(DoubleSolenoid.Value.kForward); } - public static void retract() { + public void retract() { + //forward.set(false); + //backwards.set(true); doubleSol.set(DoubleSolenoid.Value.kReverse); + doubleSol2.set(DoubleSolenoid.Value.kReverse); } - public static void stop() { + public void stop() { doubleSol.set(DoubleSolenoid.Value.kOff); + doubleSol2.set(DoubleSolenoid.Value.kOff); } + public boolean getOut(){ + return out; + } + public void toggleOut(){ + out = !out; + } } diff --git a/vendordeps/ADIS16448.json b/vendordeps/ADIS16448.json new file mode 100644 index 0000000..09aa0e8 --- /dev/null +++ b/vendordeps/ADIS16448.json @@ -0,0 +1,32 @@ +{ + "fileName": "ADIS16448.json", + "name": "ADIS16448-IMU", + "version": "2019-r2", + "uuid": "38c21ab6-aa8b-44bc-b844-8086c77f09ec", + "mavenUrls": [ + "http://maven.highcurrent.io/maven" + ], + "jsonUrl": "http://maven.highcurrent.io/vendordeps/ADIS16448.json", + "javaDependencies": [ + { + "groupId": "com.github.juchong", + "artifactId": "adis16448-java", + "version": "2019-r2" + } + ], + "jniDependencies": [], + "cppDependencies": [ + { + "groupId": "com.github.juchong", + "artifactId": "adis16448-cpp", + "version": "2019-r2", + "libName": "libadis16448imu", + "headerClassifier": "headers", + "sharedLibrary": false, + "skipInvalidPlatforms": true, + "binaryPlatforms": [ + "linuxathena" + ] + } + ] +} \ No newline at end of file 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