From 78ee3c59a9e0f17358fd2241314a5f0cf7ef374b Mon Sep 17 00:00:00 2001 From: Roman Semenov Date: Mon, 13 Mar 2023 21:04:20 -0700 Subject: [PATCH 1/2] Initial Arm implementation --- src/main/java/frc/robot/Constants.java | 48 ++++++ src/main/java/frc/robot/RobotContainer.java | 23 ++- src/main/java/frc/robot/commands/ArmTake.java | 37 +++++ .../java/frc/robot/commands/ArmThrowMid.java | 38 +++++ .../java/frc/robot/commands/ArmThrowUp.java | 38 +++++ src/main/java/frc/robot/subsystems/Arm.java | 145 ++++++++++++++++++ 6 files changed, 328 insertions(+), 1 deletion(-) create mode 100644 src/main/java/frc/robot/commands/ArmTake.java create mode 100644 src/main/java/frc/robot/commands/ArmThrowMid.java create mode 100644 src/main/java/frc/robot/commands/ArmThrowUp.java create mode 100644 src/main/java/frc/robot/subsystems/Arm.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 89e2039..283cddb 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -26,4 +26,52 @@ public static class OperatorConstants { public static final int LEFT_INTAKE = 0; public static final int XBOX_CONTROLLER_PORT = 0; public static final int LINE_BREAKER_PORT = 1; + +// arm constants +public static final int frameMotorPort = 5; +public static final int rodMotorPort = 6; +public static final int throwerMotorPort = 7; + +public static final int[] kEncoderPorts = + new int[] { 4,5, + 6,7, + 8,9}; +public static final int kEncoderPPR = 256; +public static final double kEncoderDistancePerPulse = 2.0 * Math.PI / kEncoderPPR; + +public static final double kArmOffsetRads = 0.5; +public static final double kP = 1; + +public static final double kMaxAccelerationRadPerSecSquared = 10; +public static final double kMaxVelocityRadPerSecond = 3; + +public static final double kSVolts = 1; +public static final double kGVolts = 1; +public static final double kVVoltSecondPerRad = 0.5; +public static final double kAVoltSecondSquaredPerRad = 0.1; + + } + +//public static final class ArmConstants { +// public static final int kMotorPort = 4; +// +// public static final double kP = 1; +// +// // These are fake gains; in actuality these must be determined individually for each robot +// public static final double kSVolts = 1; +// public static final double kGVolts = 1; +// public static final double kVVoltSecondPerRad = 0.5; +// public static final double kAVoltSecondSquaredPerRad = 0.1; +// +// public static final double kMaxVelocityRadPerSecond = 3; +// public static final double kMaxAccelerationRadPerSecSquared = 10; +// +// public static final int[] kEncoderPorts = new int[] {4, 5}; +// public static final int kEncoderPPR = 256; +// public static final double kEncoderDistancePerPulse = 2.0 * Math.PI / kEncoderPPR; +// +// // The offset of the arm from the horizontal in its neutral position, +// // measured from the horizontal +// public static final double kArmOffsetRads = 0.5; +//} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 5af2f56..f15c2d6 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -12,13 +12,19 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj2.command.button.POVButton; import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.commands.DriveArcade; import frc.robot.commands.DriveArcadeCustomized; import frc.robot.commands.DriveCurvature; import frc.robot.commands.DriveTank; import frc.robot.commands.GrabberOne; +import frc.robot.commands.ArmCalibrate; +import frc.robot.commands.ArmTake; +import frc.robot.commands.ArmThrowUp; +import frc.robot.commands.ArmThrowMid; import frc.robot.subsystems.BoxGrabber; +import frc.robot.subsystems.Arm; // import frc.robot.commands.GrabberIntake; // import frc.robot.commands.GrabberOne; // import frc.robot.subsystems.BoxGrabber; @@ -32,6 +38,7 @@ */ public class RobotContainer { // The robot's subsystems and commands are defined here... + Arm arm = new Arm(); BoxGrabber boxGrabber = new BoxGrabber(); Drivetrain driveTrain = new Drivetrain(); DigitalInput digitalInput = new DigitalInput(Constants.LINE_BREAKER_PORT); // add port number in constants file @@ -39,11 +46,20 @@ public class RobotContainer { XboxController xboxController = new XboxController(Constants.XBOX_CONTROLLER_PORT); + POVButton[] povButtons; + //POVButton m_povDown = new POVButton(xboxController,0); + //POVButton m_povLeft = new POVButton(xboxController,90); + //POVButton m_povUp = new POVButton(xboxController,180); + //POVButton m_povRight = new POVButton(xboxController,270); // Replace with CommandPS4Controller or CommandJoystick if needed /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { + for (int i=0; i<4; ++i) { + povButtons[i] = new POVButton(xboxController, i*90); + } + // Configure the trigger bindings configureBindings(); // driveTrain.setDefaultCommand(new DriveArcade(driveTrain, xboxController::getRightX, xboxController::getLeftY)); @@ -66,10 +82,15 @@ public RobotContainer() { * joysticks}. */ private void configureBindings() { + // Schedule `ExampleCommand` when `exampleCondition` changes to `true` new JoystickButton(xboxController, Button.kX.value).whileTrue(new GrabberOne(boxGrabber, 0.75, digitalInput)); new JoystickButton(xboxController, Button.kY.value).whileTrue(new GrabberOne(boxGrabber, 0.75, digitalInput)); + new JoystickButton(xboxController, Button.kA.value).whileTrue(new ArmCalibrate(arm)); + povButtons[0].onTrue(new ArmTake(arm)); + povButtons[1].onTrue(new ArmThrowUp(arm)); + povButtons[2].onTrue(new ArmThrowMid(arm)); // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed, // cancelling on release. @@ -81,4 +102,4 @@ private void configureBindings() { * @return the command to run in autonomous */ -} \ No newline at end of file +} diff --git a/src/main/java/frc/robot/commands/ArmTake.java b/src/main/java/frc/robot/commands/ArmTake.java new file mode 100644 index 0000000..cabd227 --- /dev/null +++ b/src/main/java/frc/robot/commands/ArmTake.java @@ -0,0 +1,37 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Arm; + +public class ArmTake extends CommandBase { + Arm arm; + + public ArmTake(Arm arm) { + this.arm = arm; + } + + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + arm.take(); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/ArmThrowMid.java b/src/main/java/frc/robot/commands/ArmThrowMid.java new file mode 100644 index 0000000..22f8995 --- /dev/null +++ b/src/main/java/frc/robot/commands/ArmThrowMid.java @@ -0,0 +1,38 @@ + +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Arm; + +public class ArmThrowMid extends CommandBase { + Arm arm; + + public ArmThrowMid(Arm arm) { + this.arm = arm; + } + + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + arm.throwUp(); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/commands/ArmThrowUp.java b/src/main/java/frc/robot/commands/ArmThrowUp.java new file mode 100644 index 0000000..a9ff1ac --- /dev/null +++ b/src/main/java/frc/robot/commands/ArmThrowUp.java @@ -0,0 +1,38 @@ + +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.commands; +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj2.command.CommandBase; +import frc.robot.subsystems.Arm; + +public class ArmThrowUp extends CommandBase { + Arm arm; + + public ArmThrowUp(Arm arm) { + this.arm = arm; + } + + + // Called when the command is initially scheduled. + @Override + public void initialize() {} + + // Called every time the scheduler runs while the command is scheduled. + @Override + public void execute() { + arm.throwUp(); + } + + // Called once the command ends or is interrupted. + @Override + public void end(boolean interrupted) {} + + // Returns true when the command should end. + @Override + public boolean isFinished() { + return false; + } +} diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java new file mode 100644 index 0000000..6ecdf9d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -0,0 +1,145 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +package frc.robot.subsystems; + +import edu.wpi.first.math.controller.ArmFeedforward; +import edu.wpi.first.math.controller.ProfiledPIDController; +import edu.wpi.first.math.trajectory.TrapezoidProfile; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax; + +import edu.wpi.first.wpilibj2.command.ProfiledPIDSubsystem; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.Constants; + +// sim dependencies +import edu.wpi.first.wpilibj.simulation.EncoderSim; + +public class Arm extends SubsystemBase { + + private class Frame extends ProfiledPIDSubsystem { + private final PWMSparkMax m_frameMotor = + new PWMSparkMax(Constants.frameMotorPort); + private final Encoder m_frameEncoder = + new Encoder(Constants.kEncoderPorts[0], Constants.kEncoderPorts[1]); + private final ArmFeedforward m_frameFeedforward = + new ArmFeedforward(Constants.kSVolts, Constants.kGVolts, + Constants.kVVoltSecondPerRad, + Constants.kAVoltSecondSquaredPerRad); + + public Frame() { + super( + new ProfiledPIDController( + Constants.kP, + 0, + 0, + new TrapezoidProfile.Constraints( + Constants.kMaxVelocityRadPerSecond, + Constants.kMaxAccelerationRadPerSecSquared)), + 0); + m_frameEncoder.setDistancePerPulse(Constants.kEncoderDistancePerPulse); + } + + @Override + public void useOutput(double output, TrapezoidProfile.State setpoint) { + // Calculate the feedforward from the sepoint + double feedforward = m_frameFeedforward.calculate(setpoint.position, + setpoint.velocity); + // Add the feedforward to the PID output to get the motor output + m_frameMotor.setVoltage(output + feedforward); + } + + @Override + public double getMeasurement() { + return m_frameEncoder.getDistance() + Constants.kArmOffsetRads; + } + // Start arm at rest in neutral position + //setGoal(Constants.kArmOffsetRads); + } + + private class Rod extends ProfiledPIDSubsystem { + private final PWMSparkMax m_rodMotor = + new PWMSparkMax(Constants.rodMotorPort); + private final Encoder m_rodEncoder = + new Encoder(Constants.kEncoderPorts[2], Constants.kEncoderPorts[3]); + private final ArmFeedforward m_rodFeedforward = + new ArmFeedforward(Constants.kSVolts, Constants.kGVolts, + Constants.kVVoltSecondPerRad, + Constants.kAVoltSecondSquaredPerRad); + + public Rod() { + super( + new ProfiledPIDController( + Constants.kP, + 0, + 0, + new TrapezoidProfile.Constraints( + Constants.kMaxVelocityRadPerSecond, + Constants.kMaxAccelerationRadPerSecSquared)), + 0); + m_rodEncoder.setDistancePerPulse(Constants.kEncoderDistancePerPulse); + } + + @Override + public void useOutput(double output, TrapezoidProfile.State setpoint) { + // Calculate the feedforward from the sepoint + double feedforward = m_rodFeedforward.calculate(setpoint.position, + setpoint.velocity); + // Add the feedforward to the PID output to get the motor output + m_rodMotor.setVoltage(output + feedforward); + } + + @Override + public double getMeasurement() { + return m_rodEncoder.getDistance() + Constants.kArmOffsetRads; + } + + } + + public Arm() { + Frame frame = new Frame(); + frame.setGoal(0); + + Rod rod = new Rod(); + rod.setGoal(0); + } + + public void take(){ + // move frame and rod to position for pickup, + // frame.setGoal(X); + // rod.setGoal(X); + // take cubicBall with claw + } + + public void throwUp(){ + // move frame and rod to position to throw up, + // throw cubicBall with claw + } + + public void throwMid(){ + // move frame and rod to position to throw mid, + // throw cubicBall with claw + } + + public void rest(){ + // move frame and rod to rest position + } + + public void calibrate() { + } + + public void stop(){ + // stop all motors (frame, rod, claw) + } + +} + + + +// @Override +// public void periodic() { +// // This method will be called once per scheduler run +// } +// } From 78a6c3d137617735142e8efec85f8571a8091130 Mon Sep 17 00:00:00 2001 From: Roman Semenov Date: Tue, 14 Mar 2023 16:32:39 -0700 Subject: [PATCH 2/2] fix: all the arm commands in one file --- src/main/java/frc/robot/Constants.java | 30 ++++----------- src/main/java/frc/robot/RobotContainer.java | 26 ++++++------- .../{ArmThrowMid.java => ArmCommand.java} | 28 ++++++++++++-- src/main/java/frc/robot/commands/ArmTake.java | 37 ------------------ .../java/frc/robot/commands/ArmThrowUp.java | 38 ------------------- src/main/java/frc/robot/subsystems/Arm.java | 13 ++----- 6 files changed, 47 insertions(+), 125 deletions(-) rename src/main/java/frc/robot/commands/{ArmThrowMid.java => ArmCommand.java} (57%) delete mode 100644 src/main/java/frc/robot/commands/ArmTake.java delete mode 100644 src/main/java/frc/robot/commands/ArmThrowUp.java diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 283cddb..c9d7a7d 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -28,6 +28,7 @@ public static class OperatorConstants { public static final int LINE_BREAKER_PORT = 1; // arm constants +//TODO all the constants are subject for further tuning and assignment public static final int frameMotorPort = 5; public static final int rodMotorPort = 6; public static final int throwerMotorPort = 7; @@ -50,28 +51,13 @@ public static class OperatorConstants { public static final double kVVoltSecondPerRad = 0.5; public static final double kAVoltSecondSquaredPerRad = 0.1; +public enum ArmCommands { + CALIBRATE, + REST, + THROW_UP, + THROW_MID, + INTAKE +} } -//public static final class ArmConstants { -// public static final int kMotorPort = 4; -// -// public static final double kP = 1; -// -// // These are fake gains; in actuality these must be determined individually for each robot -// public static final double kSVolts = 1; -// public static final double kGVolts = 1; -// public static final double kVVoltSecondPerRad = 0.5; -// public static final double kAVoltSecondSquaredPerRad = 0.1; -// -// public static final double kMaxVelocityRadPerSecond = 3; -// public static final double kMaxAccelerationRadPerSecSquared = 10; -// -// public static final int[] kEncoderPorts = new int[] {4, 5}; -// public static final int kEncoderPPR = 256; -// public static final double kEncoderDistancePerPulse = 2.0 * Math.PI / kEncoderPPR; -// -// // The offset of the arm from the horizontal in its neutral position, -// // measured from the horizontal -// public static final double kArmOffsetRads = 0.5; -//} diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index f15c2d6..5e4ffa0 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -19,10 +19,7 @@ import frc.robot.commands.DriveCurvature; import frc.robot.commands.DriveTank; import frc.robot.commands.GrabberOne; -import frc.robot.commands.ArmCalibrate; -import frc.robot.commands.ArmTake; -import frc.robot.commands.ArmThrowUp; -import frc.robot.commands.ArmThrowMid; +import frc.robot.commands.ArmCommand; import frc.robot.subsystems.BoxGrabber; import frc.robot.subsystems.Arm; // import frc.robot.commands.GrabberIntake; @@ -43,21 +40,15 @@ public class RobotContainer { Drivetrain driveTrain = new Drivetrain(); DigitalInput digitalInput = new DigitalInput(Constants.LINE_BREAKER_PORT); // add port number in constants file - XboxController xboxController = new XboxController(Constants.XBOX_CONTROLLER_PORT); - POVButton[] povButtons; - //POVButton m_povDown = new POVButton(xboxController,0); - //POVButton m_povLeft = new POVButton(xboxController,90); - //POVButton m_povUp = new POVButton(xboxController,180); - //POVButton m_povRight = new POVButton(xboxController,270); // Replace with CommandPS4Controller or CommandJoystick if needed /** The container for the robot. Contains subsystems, OI devices, and commands. */ public RobotContainer() { for (int i=0; i<4; ++i) { - povButtons[i] = new POVButton(xboxController, i*90); + povButtons[i] = new POVButton(xboxController, i * 90); } // Configure the trigger bindings @@ -87,10 +78,15 @@ private void configureBindings() { new JoystickButton(xboxController, Button.kX.value).whileTrue(new GrabberOne(boxGrabber, 0.75, digitalInput)); new JoystickButton(xboxController, Button.kY.value).whileTrue(new GrabberOne(boxGrabber, 0.75, digitalInput)); - new JoystickButton(xboxController, Button.kA.value).whileTrue(new ArmCalibrate(arm)); - povButtons[0].onTrue(new ArmTake(arm)); - povButtons[1].onTrue(new ArmThrowUp(arm)); - povButtons[2].onTrue(new ArmThrowMid(arm)); + // Arm controls + //TODO set appropriate button # for each command - 0,1,2,3 - may be inconvenient + //OR even map the actions to a different buttons like A,B,... + new JoystickButton(xboxController, Button.kA.value).whileTrue(new ArmCommand(arm,Constants.ArmCommands.CALIBRATE)); + povButtons[0].onTrue(new ArmCommand(arm, Constants.ArmCommands.INTAKE)); + povButtons[1].onTrue(new ArmCommand(arm, Constants.ArmCommands.THROW_UP)); + povButtons[2].onTrue(new ArmCommand(arm, Constants.ArmCommands.THROW_MID)); + povButtons[3].onTrue(new ArmCommand(arm, Constants.ArmCommands.REST)); + // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed, // cancelling on release. diff --git a/src/main/java/frc/robot/commands/ArmThrowMid.java b/src/main/java/frc/robot/commands/ArmCommand.java similarity index 57% rename from src/main/java/frc/robot/commands/ArmThrowMid.java rename to src/main/java/frc/robot/commands/ArmCommand.java index 22f8995..3da6acb 100644 --- a/src/main/java/frc/robot/commands/ArmThrowMid.java +++ b/src/main/java/frc/robot/commands/ArmCommand.java @@ -7,12 +7,19 @@ import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj2.command.CommandBase; import frc.robot.subsystems.Arm; +import frc.robot.Constants; + +public class ArmCommand extends CommandBase { + -public class ArmThrowMid extends CommandBase { Arm arm; + Constants.ArmCommands command; - public ArmThrowMid(Arm arm) { + public ArmCommand(Arm arm, Constants.ArmCommands comm) { this.arm = arm; + this.command = comm; + + addRequirements(arm); } @@ -23,7 +30,22 @@ public void initialize() {} // Called every time the scheduler runs while the command is scheduled. @Override public void execute() { - arm.throwUp(); + switch (command) { + case CALIBRATE: + arm.calibrate(); + break; + case INTAKE: + arm.intake(); + break; + case THROW_UP: + arm.throwUp(); + break; + case THROW_MID: + arm.throwMid(); + break; + default: + arm.rest(); + } } // Called once the command ends or is interrupted. diff --git a/src/main/java/frc/robot/commands/ArmTake.java b/src/main/java/frc/robot/commands/ArmTake.java deleted file mode 100644 index cabd227..0000000 --- a/src/main/java/frc/robot/commands/ArmTake.java +++ /dev/null @@ -1,37 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.Arm; - -public class ArmTake extends CommandBase { - Arm arm; - - public ArmTake(Arm arm) { - this.arm = arm; - } - - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - arm.take(); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/commands/ArmThrowUp.java b/src/main/java/frc/robot/commands/ArmThrowUp.java deleted file mode 100644 index a9ff1ac..0000000 --- a/src/main/java/frc/robot/commands/ArmThrowUp.java +++ /dev/null @@ -1,38 +0,0 @@ - -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package frc.robot.commands; -import edu.wpi.first.wpilibj.DigitalInput; -import edu.wpi.first.wpilibj2.command.CommandBase; -import frc.robot.subsystems.Arm; - -public class ArmThrowUp extends CommandBase { - Arm arm; - - public ArmThrowUp(Arm arm) { - this.arm = arm; - } - - - // Called when the command is initially scheduled. - @Override - public void initialize() {} - - // Called every time the scheduler runs while the command is scheduled. - @Override - public void execute() { - arm.throwUp(); - } - - // Called once the command ends or is interrupted. - @Override - public void end(boolean interrupted) {} - - // Returns true when the command should end. - @Override - public boolean isFinished() { - return false; - } -} diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java index 6ecdf9d..617fc95 100644 --- a/src/main/java/frc/robot/subsystems/Arm.java +++ b/src/main/java/frc/robot/subsystems/Arm.java @@ -106,10 +106,10 @@ public Arm() { rod.setGoal(0); } - public void take(){ + public void intake(){ // move frame and rod to position for pickup, - // frame.setGoal(X); - // rod.setGoal(X); + // frame.setGoal(X); + // rod.setGoal(X); // take cubicBall with claw } @@ -136,10 +136,3 @@ public void stop(){ } - - -// @Override -// public void periodic() { -// // This method will be called once per scheduler run -// } -// }