diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java index 65c24e8..de0d898 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/Robot.java @@ -49,8 +49,8 @@ public class Robot extends IterativeRobot private Command RRautonomousCommand; private Command RLautonomousCommand; private Command LRautonomousCommand; - private Command LLautonomousCommand; - + private Command LLautonomousCommand; + public void robotInit() { //Printing game data to riolog @@ -70,16 +70,14 @@ public class Robot extends IterativeRobot operationModeChooser.addDefault("Test", OperationMode.TEST); operationModeChooser.addObject("Competition", OperationMode.COMPETITION); SmartDashboard.putData("Operation Mode", operationModeChooser); - - - - - - - + //Initializing the angle of the arm to be + arm.setInitAngle(); + //ledLights.setAllLightsOn(false); - } catch (Exception e) { + } + catch (Exception e) + { System.err.println("An error occurred in robotInit()"); } } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabBallOutOfRobot.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabBallOutOfRobot.java new file mode 100644 index 0000000..2ab8874 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabBallOutOfRobot.java @@ -0,0 +1,35 @@ +/*----------------------------------------------------------------------------*/ +/* 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.commands; + +import edu.wpi.first.wpilibj.command.CommandGroup; + +public class GrabBallOutOfRobot extends CommandGroup { + /** + * Add your docs here. + */ + public GrabBallOutOfRobot() + { + // Add Commands here: + // e.g. addSequential(new Command1()); + // addSequential(new Command2()); + // these will run in order. + + // To run multiple commands at the same time, + // use addParallel() + // e.g. addParallel(new Command1()); + // addSequential(new Command2()); + // Command1 and Command2 will run in parallel. + + // A command group will require all of the subsystems that each member + // would require. + // e.g. if Command1 requires chassis, and Command2 requires arm, + // a CommandGroup containing them would require both the chassis and the + // arm. + } +} diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java index ffce721..962cfd1 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Arm.java @@ -28,6 +28,10 @@ public class Arm extends Subsystem implements Loop // One revolution of the 1-3 GEAR RATION ON THE ARM * 4096 ticks public static final double ENCODER_TICKS_TO_DEGREES = (36.0 / 12.0) * (36.0 / 24.0) * (34.0 / 24.0) * 4096.0 / (1.88 * Math.PI); + public double ARM_ANGLE_DEGREES = 0; + + // Defined speeds + public static final double JOYSTICK_INCHES_PER_MS_HI = 0.75; // Defined positions public static final double ZERO_POSITION_AUTON_FORWARD_INCHES = 8.0; @@ -83,6 +87,15 @@ public class Arm extends Subsystem implements Loop } } + //Set the degree to negative angle after initializing + public void setInitAngle() + { + double armAngleToHoriz = 70; + double initAngle = ENCODER_TICKS_TO_DEGREES - armAngleToHoriz; + + ARM_ANGLE_DEGREES = initAngle; + } + @Override public void initDefaultCommand() { } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java index 5edbeb9..9d977c5 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java @@ -15,6 +15,7 @@ import org.usfirst.frc4388.utility.CANTalonEncoder; import org.usfirst.frc4388.utility.ControlLoopable; import org.usfirst.frc4388.utility.PIDParams; import org.usfirst.frc4388.utility.SoftwarePIDPositionController; +import org.usfirst.frc4388.robot.subsystems.Arm; import com.ctre.phoenix.motorcontrol.FeedbackDevice; import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; @@ -32,7 +33,7 @@ import com.ctre.phoenix.motorcontrol.ControlMode; public class Wrist extends Subsystem { //Control Mode Array - public static enum WristControlMode {PID, JOYSTICK_MANUAL}; + public static enum WristControlMode {PID, JOYSTICK_MANUAL, GRAB_BALL}; //Motor Controllers private ArrayList motorControllers = new ArrayList(); @@ -60,18 +61,20 @@ public class Wrist extends Subsystem public static final double MIN_POSITION_INCHES = 0.0; public static final double MAX_POSITION_INCHES = 83.4; - public static final double MIN_ANGLE_DEGREES = 0.0; ////FIND ANGLE VALUES - public static final double MAX_ANGLE_DEGREES = 0.0; + public static final double MIN_ANGLE_DEGREES = -3; ////FIND ANGLE VALUES + public static final double MAX_ANGLE_DEGREES = 3; public static final double JOYSTICK_INCHES_PER_MS_HI = 0.75; public static final double JOYSTICK_INCHES_PER_MS_LO = JOYSTICK_INCHES_PER_MS_HI/3.68 * 0.8; public static final double JOYSTICK_Degrees_PER_MS_LO = JOYSTICK_INCHES_PER_MS_HI/3.68 * 0.8; + + public double armAngleDegrees = Robot.arm.ARM_ANGLE_DEGREES; //Misc private WristControlMode wristControlMode = WristControlMode.JOYSTICK_MANUAL; private boolean isFinished; private double targetPositionInchesPID = 0; - private double targetAngleDegreesPID = 0; + private double targetAngleDegreesPID = -(180 - armAngleDegrees); private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO; private double joystickDegreesPerMs = JOYSTICK_Degrees_PER_MS_LO;