diff --git a/2019robot/build.gradle b/2019robot/build.gradle index f15e84c..86010b5 100644 --- a/2019robot/build.gradle +++ b/2019robot/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.2.1" } def ROBOT_MAIN_CLASS = "org.usfirst.frc4388.robot.Main" diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java index 64e015f..564fd0d 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java @@ -17,9 +17,8 @@ public class RobotMap { public static final int DRIVETRAIN_RIGHT_MOTOR1_CAN_ID = 4; public static final int DRIVETRAIN_RIGHT_MOTOR2_CAN_ID = 5; - //carriage motors - public static final int CARRIAGE_LEFT_MOTOR_CAN_ID = 8; - public static final int CARRIAGE_RIGHT_MOTOR_CAN_ID = 9; + //Intake motors + public static final int INTAKE_BELT_DRIVE_CAN_ID = 8; //Elevator Motors public static final int ELEVATOR_MOTOR1_ID = 6; diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SetIntakeSpeed.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SetIntakeSpeed.java new file mode 100644 index 0000000..b188086 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/SetIntakeSpeed.java @@ -0,0 +1,44 @@ +package org.usfirst.frc4388.robot.commands; + +import org.usfirst.frc4388.robot.Robot; + +import org.usfirst.frc4388.robot.subsystems.BallIntake; + +import edu.wpi.first.wpilibj.command.Command; + +/** + * + */ +public class SetIntakeSpeed extends Command { + + private double WheelSpeed; + + // Constructor with speed + public SetIntakeSpeed(double WheelSpeed) { + this.WheelSpeed = WheelSpeed; + requires(Robot.BallIntake); + } + + // Called just before this Command runs the first time + protected void initialize() { + Robot.BallIntake.setWheelSpeed(WheelSpeed); + } + + // Called repeatedly when this Command is scheduled to run + protected void execute() { + } + + // Make this return true when this Command no longer needs to run execute() + protected boolean isFinished() { + return true; + } + + // Called once after isFinished returns true + protected void end() { + } + + // Called when another command which requires one or more of the same + // subsystems is scheduled to run + protected void interrupted() { + } +} \ No newline at end of file diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/BallIntake.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/BallIntake.java index 7f60eae..0fe50cc 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/BallIntake.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/BallIntake.java @@ -1,24 +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 org.usfirst.frc4388.robot.subsystems; +import org.usfirst.frc4388.robot.RobotMap; +import org.usfirst.frc4388.robot.commands.*; +import org.usfirst.frc4388.robot.subsystems.Drive.DriveControlMode; +import org.usfirst.frc4388.utility.CANTalonEncoder; +import org.usfirst.frc4388.utility.ControlLoopable; + import edu.wpi.first.wpilibj.command.Subsystem; +import org.usfirst.frc4388.robot.OI; +import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import com.ctre.phoenix.motorcontrol.ControlMode; +import com.ctre.phoenix.motorcontrol.FeedbackDevice; + + /** - * Add your docs here. + * */ public class BallIntake 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()); - } + private WPI_TalonSRX BallIntakeMain; + public static enum BallIntakeControlMode { JOYSTICK, MP_STRAIGHT, HOLD, MANUAL}; + public static final double BALL_INTAKE_SPEED = 0.40; + public static final double BALL_EXTAKE_SPEED = -1.0; + public static final double CUBE_STOP_SPEED = 0; + /////^^^^^^^^^ replace this line with the modes we need + + + private boolean isFinished; + //private CarriageControlMode controlMode = CarriageControlMode.JOYSTICK; + + + + + public BallIntake() { + try { + BallIntakeMain = new WPI_TalonSRX(RobotMap.INTAKE_BELT_DRIVE_CAN_ID); + //\][carriageLeft.set(CurrentLimit, value); + + } + catch (Exception e) { + System.err.println("An error occurred in the Ball Intake constructor"); + + + } + } + + public void setWheelSpeed(double speed) { + BallIntakeMain.set(-speed); + } + + + @Override + public void periodic() { + + } + public void initDefaultCommand() { + } }