From b64c23c62096da6c15fe60da7c2cf9e41aa9ab58 Mon Sep 17 00:00:00 2001 From: mayabartels Date: Fri, 1 Feb 2019 18:32:21 -0800 Subject: [PATCH 1/7] Left to right motor name change --- .../main/java/org/usfirst/frc4388/robot/RobotMap.java | 2 +- .../java/org/usfirst/frc4388/robot/subsystems/Wrist.java | 9 +++++++-- 2 files changed, 8 insertions(+), 3 deletions(-) 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 d13fe44..855a5cd 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java @@ -18,7 +18,7 @@ public class RobotMap { public static final int DRIVETRAIN_RIGHT_MOTOR2_CAN_ID = 5; //carriage motors - public static final int WRIST_LEFT_MOTOR_CAN_ID = 6; + public static final int WRIST_RIGHT_MOTOR_CAN_ID = 6; public static final int INTAKE_MOTOR = 7; //Arm Motors 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 4ee0897..a8e3ff7 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 @@ -94,7 +94,7 @@ public class Wrist extends Subsystem try { //PID wrist encoder and talon - wristRight = new CANTalonEncoder(RobotMap.WRIST_LEFT_MOTOR_CAN_ID, WRIST_ENCODER_TICKS_TO_DEGREES, FeedbackDevice.QuadEncoder); + wristRight = new CANTalonEncoder(RobotMap.WRIST_RIGHT_MOTOR_CAN_ID, WRIST_ENCODER_TICKS_TO_DEGREES, FeedbackDevice.QuadEncoder); } catch(Exception e) { @@ -222,7 +222,7 @@ public class Wrist extends Subsystem { double joystickSpeed; - joystickSpeed = -Robot.oi.getOperatorController().getLeftYAxis(); + joystickSpeed = -Robot.oi.getOperatorController().getRightYAxis(); setSpeedJoystick(joystickSpeed); } @@ -242,6 +242,11 @@ public class Wrist extends Subsystem updatePositionPID(targetAngleDegreesBallIn); } + public void controlPIDBallOut() + { + + } + public synchronized boolean isFinished() { return isFinished; From 00153988041b9457fce68bd68fe3370e68eacecc Mon Sep 17 00:00:00 2001 From: mayabartels Date: Fri, 1 Feb 2019 18:51:24 -0800 Subject: [PATCH 2/7] Added to PID loop for wrist --- .../org/usfirst/frc4388/robot/RobotMap.java | 11 ++------ .../frc4388/robot/subsystems/Wrist.java | 25 ++++++++++++++++++- 2 files changed, 26 insertions(+), 10 deletions(-) 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 855a5cd..f9a75b0 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/RobotMap.java @@ -2,18 +2,16 @@ package org.usfirst.frc4388.robot; -public class RobotMap { +public class RobotMap +{ // USB Port IDs public static final int DRIVER_JOYSTICK_1_USB_ID = 0; public static final int OPERATOR_JOYSTICK_1_USB_ID = 1; - // MOTORS - public static final int DRIVETRAIN_LEFT_MOTOR1_CAN_ID = 2; public static final int DRIVETRAIN_LEFT_MOTOR2_CAN_ID = 3; - public static final int DRIVETRAIN_RIGHT_MOTOR1_CAN_ID = 4; public static final int DRIVETRAIN_RIGHT_MOTOR2_CAN_ID = 5; @@ -29,8 +27,6 @@ public class RobotMap { public static final int CLIMBER_LEFT = 12; public static final int CLIMBER_RIGHT = 13; - - // Pneumatics public static final int DRIVETRAIN_SPEEDSHIFT_PCM_ID = 0; public static final int DRIVETRAIN_SPEEDSHIFT2_PCM_ID = 1; @@ -40,7 +36,4 @@ public class RobotMap { public static final int LOWER_HATCH_INTAKE_PCM_ID = 5; public static final int END_EFECTOR_EXPAND_PCM_ID = 6; public static final int END_EFECTOR_CONTRACT_PCM_ID = 7; - - - } 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 a8e3ff7..a84e0bf 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 @@ -79,6 +79,8 @@ public class Wrist extends Subsystem public static final double jumpBarAngle = 50; //hard limit switch? public static final double armAngleForPIDSwitch = -45; ///Change values + + public static final boolean ballIntakeOut = true; //Misc private WristControlMode wristControlMode = WristControlMode.JOYSTICK_MANUAL; @@ -185,14 +187,21 @@ public class Wrist extends Subsystem synchronized (Wrist.this) { switch(getWristControlMode() ) { case PID: + if(armAngleDegrees > armAngleForPIDSwitch) { controlPID(); } else if(armAngleDegrees <= armAngleForPIDSwitch) { + if(ballIntakeOut) + { + } + else + { + } } - controlPID(); + break; case JOYSTICK_MANUAL: controlManualWithJoystick(); @@ -240,11 +249,25 @@ public class Wrist extends Subsystem public void controlPIDBallIn() { updatePositionPID(targetAngleDegreesBallIn); + //Needs limit to lift up (so it can get over bar) in command group } public void controlPIDBallOut() { + updatePositionPID(targetAngleDegreesBallOut); + //Needs to be tuned a lot + } + public void controlPIDHatchIn() + { + updatePositionPID(targetAngleDegreesHatchIn); + //Needs limit to lift up (so it can get over bar) in command group + } + + public void controlPIDHatchOut() + { + updatePositionPID(targetAngleDegreesHatchOut); + //Needs to be tuned a lot } public synchronized boolean isFinished() From 31b7e9ed0d2052f41f0cce98ec74a4c6fab4b206 Mon Sep 17 00:00:00 2001 From: mayabartels Date: Fri, 1 Feb 2019 20:01:17 -0800 Subject: [PATCH 3/7] Wrist PID update --- .../frc4388/robot/subsystems/Wrist.java | 27 ++++++++++++++----- 1 file changed, 21 insertions(+), 6 deletions(-) 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 a84e0bf..e908a1e 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 @@ -36,7 +36,7 @@ import com.ctre.phoenix.motorcontrol.ControlMode; public class Wrist extends Subsystem { //Control Mode Array - public static enum WristControlMode {PID, JOYSTICK_MANUAL, FLIP_INTAKE, GRAB_BALL}; + public static enum WristControlMode {PID, JOYSTICK_MANUAL, GRAB_BALL}; //Motor Controllers private ArrayList motorControllers = new ArrayList(); @@ -72,12 +72,13 @@ public class Wrist extends Subsystem public double armAngleDegrees = Robot.arm.ARM_ANGLE_DEGREES; - public static final double targetAngleDegreesBallIn = -45; ///Change values + public static final double targetAngleDegreesBallIn = 180; //only have to flip intake and go down to get ball + ///Change values public static final double targetAngleDegreesBallOut = 360; - public static final double targetAngleDegreesHatchIn = 55; + public static final double targetAngleDegreesHatchIn = 130; public static final double targetAngleDegreesHatchOut = 0; - public static final double jumpBarAngle = 50; //hard limit switch? + public static final double jumpBarAngle = -50; //hard limit switch? public static final double armAngleForPIDSwitch = -45; ///Change values public static final boolean ballIntakeOut = true; @@ -187,7 +188,6 @@ public class Wrist extends Subsystem synchronized (Wrist.this) { switch(getWristControlMode() ) { case PID: - if(armAngleDegrees > armAngleForPIDSwitch) { controlPID(); @@ -196,12 +196,27 @@ public class Wrist extends Subsystem { if(ballIntakeOut) { + if(armAngleDegrees > targetAngleDegreesBallIn) + { + controlPIDBallIn(); + } + else + { + controlPIDBallOut(); + } } else { + if(armAngleDegrees > targetAngleDegreesHatchIn) + { + controlPIDBallIn(); + } + else + { + controlPIDHatchOut(); + } } } - break; case JOYSTICK_MANUAL: controlManualWithJoystick(); From 9cc06f7825ab3020b475bd446181aa5932386b1f Mon Sep 17 00:00:00 2001 From: mayabartels Date: Fri, 1 Feb 2019 20:44:07 -0800 Subject: [PATCH 4/7] Wrist PID update --- .../robot/commands/GrabBallOutOfRobot.java | 5 +++ .../frc4388/robot/commands/WristAvoidBar.java | 44 +++++++++++++++++++ .../frc4388/robot/subsystems/Wrist.java | 10 +---- 3 files changed, 50 insertions(+), 9 deletions(-) create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristAvoidBar.java 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 index 2ab8874..5392859 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabBallOutOfRobot.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabBallOutOfRobot.java @@ -7,6 +7,7 @@ package org.usfirst.frc4388.robot.commands; +import org.usfirst.frc4388.robot.Robot; import edu.wpi.first.wpilibj.command.CommandGroup; public class GrabBallOutOfRobot extends CommandGroup { @@ -15,6 +16,10 @@ public class GrabBallOutOfRobot extends CommandGroup { */ public GrabBallOutOfRobot() { + if(Robot.wrist.armAngleDegrees <= Robot.wrist.jumpBarArmAngle && Robot.wrist.armAngleDegrees >= Robot.wrist.jumpBarArmAngle) + { + + } // Add Commands here: // e.g. addSequential(new Command1()); // addSequential(new Command2()); diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristAvoidBar.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristAvoidBar.java new file mode 100644 index 0000000..7e919ce --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristAvoidBar.java @@ -0,0 +1,44 @@ +/*----------------------------------------------------------------------------*/ +/* 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.Command; + +public class WristAvoidBar extends Command { + public WristAvoidBar() { + // Use requires() here to declare subsystem dependencies + // eg. requires(chassis); + } + + // 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/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/subsystems/Wrist.java index e908a1e..6fc8e65 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 @@ -78,7 +78,7 @@ public class Wrist extends Subsystem public static final double targetAngleDegreesHatchIn = 130; public static final double targetAngleDegreesHatchOut = 0; - public static final double jumpBarAngle = -50; //hard limit switch? + public final double jumpBarArmAngle = -50; public static final double armAngleForPIDSwitch = -45; ///Change values public static final boolean ballIntakeOut = true; @@ -105,14 +105,6 @@ public class Wrist extends Subsystem } } - //Flipping the intake to the other side - public void flipIntake() - { - double currentWristAngle = wristRight.getPositionWorld(); - - - } - //Method for setting the control mode for the wrist private synchronized void setWristControlMode(WristControlMode controlMode) { From a728e1dda3843528ed26ec6db2a931d444dc4adf Mon Sep 17 00:00:00 2001 From: mayabartels Date: Sat, 2 Feb 2019 15:02:19 -0800 Subject: [PATCH 5/7] Added commands for grabbing ball and hatch out of robot --- .../frc4388/robot/commands/FlipIntake.java | 53 +++++++++++++++++++ .../robot/commands/GrabBallOutOfRobot.java | 41 +++++++------- .../robot/commands/GrabHatchOffWall.java | 36 +++++++++++++ .../commands/GrabHatchOutOfFloorIntake.java | 37 +++++++++++++ .../robot/commands/RunWristMotorJumpBar.java | 53 +++++++++++++++++++ ...WristAvoidBar.java => StopWristMotor.java} | 24 ++++++--- .../frc4388/robot/subsystems/Wrist.java | 14 +++++ 7 files changed, 230 insertions(+), 28 deletions(-) create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/FlipIntake.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabHatchOffWall.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabHatchOutOfFloorIntake.java create mode 100644 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/RunWristMotorJumpBar.java rename 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/{WristAvoidBar.java => StopWristMotor.java} (77%) diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/FlipIntake.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/FlipIntake.java new file mode 100644 index 0000000..fc232a9 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/FlipIntake.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 org.usfirst.frc4388.robot.commands; + +import org.usfirst.frc4388.robot.*; + +import edu.wpi.first.wpilibj.command.Command; + +public class FlipIntake extends Command +{ + public FlipIntake() + { + requires(Robot.wrist); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() + { + Robot.wrist.controlPIDFlipIntake(); + } + + // 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/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabBallOutOfRobot.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabBallOutOfRobot.java index 5392859..6aa12c0 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabBallOutOfRobot.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabBallOutOfRobot.java @@ -8,33 +8,34 @@ package org.usfirst.frc4388.robot.commands; import org.usfirst.frc4388.robot.Robot; -import edu.wpi.first.wpilibj.command.CommandGroup; -public class GrabBallOutOfRobot extends CommandGroup { - /** - * Add your docs here. - */ +import edu.wpi.first.wpilibj.command.CommandGroup; +import edu.wpi.first.wpilibj.command.WaitCommand; + +/** +* Add your docs here. +*/ + +public class GrabBallOutOfRobot extends CommandGroup +{ public GrabBallOutOfRobot() { + //Add rest of sequentials for this command group + + addSequential(new FlipIntake()); + //Move Arm until bar jump angle + + //Jump bar in robot if(Robot.wrist.armAngleDegrees <= Robot.wrist.jumpBarArmAngle && Robot.wrist.armAngleDegrees >= Robot.wrist.jumpBarArmAngle) { - + addParallel(new WaitCommand(2)); + addSequential(new RunWristMotorJumpBar()); + addSequential(new StopWristMotor()); } - // 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. + //continue arm movement until ball is in intake ///SENSOR for ball in robot - // 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. + //Move ball out of robot to 360 degrees + //move arm and wrist in parallel } } diff --git a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabHatchOffWall.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabHatchOffWall.java new file mode 100644 index 0000000..d7f0377 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabHatchOffWall.java @@ -0,0 +1,36 @@ +/*----------------------------------------------------------------------------*/ +/* 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; + +/** +* Add your docs here. +*/ + +public class GrabHatchOffWall extends CommandGroup +{ + public GrabHatchOffWall() { + // 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/commands/GrabHatchOutOfFloorIntake.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabHatchOutOfFloorIntake.java new file mode 100644 index 0000000..b6c100c --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/GrabHatchOutOfFloorIntake.java @@ -0,0 +1,37 @@ +/*----------------------------------------------------------------------------*/ +/* 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; + +/** +* Add your docs here. +*/ + +public class GrabHatchOutOfFloorIntake extends CommandGroup +{ + public GrabHatchOutOfFloorIntake() + { + // 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/commands/RunWristMotorJumpBar.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/RunWristMotorJumpBar.java new file mode 100644 index 0000000..6fe8b19 --- /dev/null +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/RunWristMotorJumpBar.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 org.usfirst.frc4388.robot.commands; + +import org.usfirst.frc4388.robot.*; + +import edu.wpi.first.wpilibj.command.Command; + +public class RunWristMotorJumpBar extends Command +{ + public RunWristMotorJumpBar() + { + requires(Robot.wrist); + } + + // Called just before this Command runs the first time + @Override + protected void initialize() + { + Robot.wrist.jumpBar(); + } + + // 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/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristAvoidBar.java b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StopWristMotor.java similarity index 77% rename from 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristAvoidBar.java rename to 2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StopWristMotor.java index 7e919ce..164e14f 100644 --- a/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/WristAvoidBar.java +++ b/2019robot/src/main/java/org/usfirst/frc4388/robot/commands/StopWristMotor.java @@ -7,33 +7,41 @@ package org.usfirst.frc4388.robot.commands; +import org.usfirst.frc4388.robot.*; + import edu.wpi.first.wpilibj.command.Command; -public class WristAvoidBar extends Command { - public WristAvoidBar() { - // Use requires() here to declare subsystem dependencies - // eg. requires(chassis); +public class StopWristMotor extends Command +{ + public StopWristMotor() + { + requires(Robot.wrist); } // Called just before this Command runs the first time @Override - protected void initialize() { + protected void initialize() + { + Robot.wrist.stopMotor(); } // Called repeatedly when this Command is scheduled to run @Override - protected void execute() { + protected void execute() + { } // Make this return true when this Command no longer needs to run execute() @Override - protected boolean isFinished() { + protected boolean isFinished() + { return false; } // Called once after isFinished returns true @Override - protected void end() { + protected void end() + { } // Called when another command which requires one or more of the same 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 6fc8e65..5f3b0b1 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 @@ -26,6 +26,7 @@ import com.ctre.phoenix.motorcontrol.LimitSwitchSource; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.SensorCollection; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; +import edu.wpi.first.wpilibj.command.WaitCommand; import com.ctre.phoenix.motorcontrol.ControlMode; @@ -105,6 +106,19 @@ public class Wrist extends Subsystem } } + //Jump bar by putting power to the motors for a specific amount of time + //Jump bar output + public void jumpBar() + { + wristRight.set(0.8); + } + + //Stop wrist motor + public void stopMotor() + { + wristRight.set(0); + } + //Method for setting the control mode for the wrist private synchronized void setWristControlMode(WristControlMode controlMode) { From 3e2b0dc235582dd029b508e951b057123fc42048 Mon Sep 17 00:00:00 2001 From: mayabartels Date: Thu, 14 Feb 2019 15:52:14 -0800 Subject: [PATCH 6/7] Limit Switch Added to Wrist --- .../usfirst/frc4388/robot/subsystems/Wrist.java | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) 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 5f3b0b1..bc9fd7c 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 @@ -28,6 +28,10 @@ import com.ctre.phoenix.motorcontrol.SensorCollection; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; import edu.wpi.first.wpilibj.command.WaitCommand; +import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; +import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource; +import com.ctre.phoenix.motorcontrol.LimitSwitchSource; + import com.ctre.phoenix.motorcontrol.ControlMode; /** @@ -83,6 +87,9 @@ public class Wrist extends Subsystem public static final double armAngleForPIDSwitch = -45; ///Change values public static final boolean ballIntakeOut = true; + + //control mode for joystick control + private DriveControlMode controlMode = DriveControlMode.JOYSTICK; //Misc private WristControlMode wristControlMode = WristControlMode.JOYSTICK_MANUAL; @@ -93,12 +100,20 @@ public class Wrist extends Subsystem private double joystickInchesPerMs = JOYSTICK_INCHES_PER_MS_LO; private double joystickDegreesPerMs = JOYSTICK_Degrees_PER_MS_LO; + LimitSwitchSource limitSwitchSource; + private boolean pressed; + SensorCollection isPressed; + public Wrist() { try { //PID wrist encoder and talon - wristRight = new CANTalonEncoder(RobotMap.WRIST_RIGHT_MOTOR_CAN_ID, WRIST_ENCODER_TICKS_TO_DEGREES, FeedbackDevice.QuadEncoder); + wristRight = new CANTalonEncoder(RobotMap.WRIST_RIGHT_MOTOR_CAN_ID, WRIST_ENCODER_TICKS_TO_DEGREES, FeedbackDevice.QuadEncoder); + + //Limit Switch + wristRight.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + wristRight.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); } catch(Exception e) { From d7ce32e5c8858092ca8a5609c053f760fc101fe2 Mon Sep 17 00:00:00 2001 From: mayabartels Date: Thu, 14 Feb 2019 15:55:12 -0800 Subject: [PATCH 7/7] Slight edit --- .../usfirst/frc4388/robot/subsystems/Arm.java | 543 ++++++++++++------ .../frc4388/robot/subsystems/Wrist.java | 2 - 2 files changed, 363 insertions(+), 182 deletions(-) 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 a8bedac..0278e79 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 @@ -1,240 +1,423 @@ package org.usfirst.frc4388.robot.subsystems; import java.util.ArrayList; -import java.lang.Math; +import org.usfirst.frc4388.controller.XboxController; import org.usfirst.frc4388.robot.Constants; import org.usfirst.frc4388.robot.Robot; 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.Loop; -import org.usfirst.frc4388.utility.MPTalonPIDController; +import org.usfirst.frc4388.utility.ControlLoopable; import org.usfirst.frc4388.utility.PIDParams; -import org.usfirst.frc4388.utility.TalonSRXEncoder; +import org.usfirst.frc4388.utility.SoftwarePIDPositionController; import com.ctre.phoenix.motorcontrol.ControlMode; + +import edu.wpi.first.wpilibj.DigitalInput; +import edu.wpi.first.wpilibj.Encoder; +import edu.wpi.first.wpilibj.PIDController; +import edu.wpi.first.wpilibj.SpeedControllerGroup; +import edu.wpi.first.wpilibj.command.Subsystem; +import edu.wpi.first.wpilibj.drive.DifferentialDrive; +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; + import com.ctre.phoenix.motorcontrol.FeedbackDevice; -import com.ctre.phoenix.motorcontrol.can.TalonSRX; -import com.ctre.phoenix.motorcontrol.LimitSwitchSource; import com.ctre.phoenix.motorcontrol.LimitSwitchNormal; import com.ctre.phoenix.motorcontrol.RemoteLimitSwitchSource; +import com.ctre.phoenix.motorcontrol.LimitSwitchSource; import com.ctre.phoenix.motorcontrol.NeutralMode; import com.ctre.phoenix.motorcontrol.SensorCollection; import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX; -import edu.wpi.first.wpilibj.command.Subsystem; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; - -public class Arm extends Subsystem implements Loop +public class Arm extends Subsystem implements ControlLoopable { - private static Arm instance; + //PID encoder and motor + private CANTalonEncoder armMotor; + //private WPI_TalonSRX ArmLeft; - public static enum ArmControlMode {PID, JOYSTICK_MANUAL }; - public static final double DEGREE_START_OFFSET = 70; - - public static final double RADIUS_OF_ARM = 43.3; - public static final double OFF_SET_BELOW = 49.3; - - // One revolution of the 1-3 GEAR RATION ON THE ARM * 4096 ticks - public static final double ENCODER_TICKS_TO_DEGREES = ((4096/360)*(1/3))-(DEGREE_START_OFFSET); - public static final double X_POSITION_MATH = ((RADIUS_OF_ARM)*(Math.cos(ENCODER_TICKS_TO_DEGREES)))+(OFF_SET_BELOW); - public static final double Y_POSITION_MATH = (RADIUS_OF_ARM)*(Math.sin(ENCODER_TICKS_TO_DEGREES)); + //PID controller Max Scale + private SoftwarePIDPositionController pidPositionControllerMaxScale; + //private PIDParams PositionPIDParamsMaxScale = new PIDParams(2.0, 0.0, 0.0); + private PIDParams PositionPMaxScale; - public double ARM_ANGLE_DEGREES = 0; - - // Defined speeds - public static final double JOYSTICK_INCHES_PER_MS_HI = 0.75; + //PID controller Max Scale + private SoftwarePIDPositionController pidPositionControllerLowScale; + //private PIDParams PositionPIDParamsLowScale = new PIDParams(2.0, 0.0, 0.0); + private PIDParams PositionPLowScale; - // Defined positions - public static final double MIN_POSITION_INCHES = 0.0; - public static final double MAX_POSITION_INCHES = 83.4; - public static final double AFTER_INTAKE_POSITION_INCHES = 4.0; - - - LimitSwitchSource limitSwitchSource; - // Motion profile max velocities and accel times - public static final double MP_MAX_VELOCITY_INCHES_PER_SEC = 60; - public static final double MP_T2 = 150; // Fast = 150 + //PID controller Max Scale + private SoftwarePIDPositionController pidPositionControllerSwitch; + //private PIDParams PositionPIDParamsSwitch = new PIDParams(2.0, 0.0, 0.0); + private PIDParams PositionPSwitch; - // Motor controllers - private ArrayList motorControllers = new ArrayList(); + //PID controller Max Scale + private SoftwarePIDPositionController pidPositionControllerLowest; + //private PIDParams PositionPIDParamsLowest = new PIDParams(2.0, 0.0, 0.0); + private PIDParams PositionPLowest; - private CANTalonEncoder motor1; - private TalonSRX motor2; + //PID target + private double targetPPosition; + + //Encoder ticks to inches for encoders + public static final double ENCODER_TICKS_TO_INCHES = Constants.kArmEncoderTicksPerInch; + + //control mode for joystick control + private DriveControlMode controlMode = DriveControlMode.JOYSTICK; + + private double periodMs; + + //Non Linear Joystick + public static final double STICK_DEADBAND = 0.02; + public static final double MOVE_NON_LINEARITY = 1.0; + private int moveNonLinear = 0; + private double moveScale = 1.0; + private double moveTrim = 0.0; - // PID controller and params - private MPTalonPIDController mpController; - - public static int PID_SLOT = 0; - public static int MP_SLOT = 1; - - private PIDParams mpPIDParams = new PIDParams(0.2, 0.0, 0.0, 0.0, 0.005, 0.0); - public static final double KF_UP = 0.005; - public static final double KF_DOWN = 0.0; - public static final double PID_ERROR_INCHES = 1.0; - private long periodMs = (long)(Constants.kLooperDt * 1000.0); - - - // Misc - public static final double AUTO_ZERO_MOTOR_CURRENT = 4.0; private boolean isFinished; - private ArmControlMode armControlMode = ArmControlMode.JOYSTICK_MANUAL; - private double targetPositionInchesPID = 0; - private boolean firstMpPoint; + private DifferentialDrive m_drive; + //private LimitSwitchSource limitSwitch = new DigitalInput(1); + LimitSwitchSource limitSwitchSource; - public Arm() { - try { - motor1 = new CANTalonEncoder(RobotMap.ARM_MOTOR1_ID, ENCODER_TICKS_TO_DEGREES, FeedbackDevice.QuadEncoder); - //motor2 = CANTallon.createPermanentSlaveTalon(RobotMap.ARM_MOTOR_2_CAN_ID, RobotMap.ELEVATOR_MOTOR_1_CAN_ID); + public boolean pressed; + SensorCollection isPressed; + + public boolean armControlMode = false; + // Motor controllers + //private ArrayList motorControllers = new ArrayList(); + + public Arm() + { + try + { + //PID Arm encoder and talon + armMotor = new CANTalonEncoder(RobotMap.ARM_MOTOR1_ID, ENCODER_TICKS_TO_INCHES, FeedbackDevice.QuadEncoder); + //ArmLeft = new WPI_TalonSRX(RobotMap.ARM_MOTOR2_ID); - motor1.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); - motor1.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); - } - catch (Exception e) { - System.err.println("An error occurred in the DriveTrain constructor"); - } - } - @Override - public void initDefaultCommand() { - } - - public void resetZeroPosition(double position) { - mpController.resetZeroPosition(position); - } - - private synchronized void setArmControlMode(ArmControlMode controlMode) { - this.armControlMode = controlMode; - } - - private synchronized ArmControlMode getArmControlMode() { - return this.armControlMode; - } + //ArmMotor.setInverted(false); + //Setting left Arm motor as follower + //ArmLeft.set(ControlMode.Follower, ArmMotor.getDeviceID()); + //ArmLeft.setInverted(false); + //ArmLeft.setNeutralMode(NeutralMode.Brake); + armMotor.setNeutralMode(NeutralMode.Brake); + armMotor.setSensorPhase(true); + //Limit Switch Left + //ArmLeft.overrideLimitSwitchesEnable(true); + //ArmLeft.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + //ArmLeft.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + + //Limit Switch Right + //ArmMotor.overrideLimitSwitchesEnable(true); + //ArmMotor.configForwardLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + //ArmMotor.configReverseLimitSwitchSource(limitSwitchSource, LimitSwitchNormal.NormallyOpen, 0); + + + //Change This boi + + // ArmMotor.configForwardSoftLimitThreshold(10000, 0); //right here + //ArmMotor.configReverseSoftLimitThreshold(5, 0); + //ArmMotor.configForwardSoftLimitEnable(true, 0); + //ArmMotor.configReverseSoftLimitEnable(true, 0); + + //sos + //ArmMotor.enableLimitSwitch(true, true); - - - public void setSpeedJoystick(double speed) { - motor1.set(ControlMode.PercentOutput, speed); - setArmControlMode(ArmControlMode.JOYSTICK_MANUAL); + + + + + } + catch(Exception e) + { + System.err.println("You thought the code would work, but it was me, error. An error occurred in the Arm Construtor"); + } + } + + private double nonlinearStickCalcPositive(double move, double moveNonLinearity) + { + return Math.sin(Math.PI / 2.0 * moveNonLinearity * move) / Math.sin(Math.PI / 2.0 * moveNonLinearity); } - - public void setPositionPID(double targetPositionInches) { - mpController.setPIDSlot(PID_SLOT); - updatePositionPID(targetPositionInches); - setArmControlMode(ArmControlMode.PID); - setFinished(false); + + private double nonlinearStickCalcNegative(double move, double moveNonLinearity) + { + return Math.asin(moveNonLinearity * move) / Math.asin(moveNonLinearity); } - - public void updatePositionPID(double targetPositionInches) { - targetPositionInchesPID = limitPosition(targetPositionInches); - double startPositionInches = motor1.getPositionWorld()*Y_POSITION_MATH; - mpController.setTarget(targetPositionInchesPID, targetPositionInchesPID > startPositionInches ? KF_UP : KF_DOWN); - } - - - private double limitPosition(double targetPosition) { - if (targetPosition < MIN_POSITION_INCHES) { - return MIN_POSITION_INCHES; - } - else if (targetPosition > MAX_POSITION_INCHES) { - return MAX_POSITION_INCHES; + + private boolean inDeadZone(double input) + { + boolean inDeadZone; + if (Math.abs(input) < STICK_DEADBAND) + { + inDeadZone = true; + } + else + { + inDeadZone = false; } - return targetPosition; + return inDeadZone; } - - @Override - public void onStart(double timestamp) { - mpController.setPIDSlot(PID_SLOT); - mpController.setPIDSlot(PID_SLOT); + + private double limitValue(double value) + { + if (value > 1.0) + { + value = 1.0; + } + else if (value < -1.0) + { + value = -1.0; + } + return value; } + + public double adjustJoystickSensitivity(double scale, double trim, double move, int nonLinearFactor, double wheelNonLinearity) + { + if (inDeadZone(move)) + { + return 0; + } - @Override - public void onStop(double timestamp) { - // TODO Auto-generated method stub - - } + move += trim; + move *= scale; + move = limitValue(move); - @Override - public void onLoop(double timestamp) { - synchronized (Arm.this) { - switch( getArmControlMode() ) { - case PID: - controlPidWithJoystick(); - break; - case JOYSTICK_MANUAL: - controlManualWithJoystick(); - break; - default: - break; - - + int iterations = Math.abs(nonLinearFactor); + for (int i = 0; i < iterations; i++) + { + if (nonLinearFactor > 0) + { + move = nonlinearStickCalcPositive(move, wheelNonLinearity); + } + else + { + move = nonlinearStickCalcNegative(move, wheelNonLinearity); } } + return move; } + + public void setArmMode() + { + setControlMode(DriveControlMode.JOYSTICK); + } + + public void resetArmEncoder() + { + armMotor.setSelectedSensorPosition(0, 0, 0); + } + + public void moveArmXbox() + { + double moveArmInput; + double armSafeZone = 15; + + double armPos = getEncoderArmPosition(); + + moveArmInput = Robot.oi.getOperatorController().getLeftYAxis(); + + //double moveArmSensitivity = adjustJoystickSensitivity(moveScale, moveTrim, moveArmInput, moveNonLinear, MOVE_NON_LINEARITY); + + boolean holdButtonPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.A_BUTTON); + boolean armTuningPressed = Robot.oi.getOperatorJoystick().getRawButton(XboxController.Y_BUTTON); + + if(armTuningPressed == true) + { + armMotor.set(moveArmInput * 0.5); + } + else if(armTuningPressed == false) + { + armMotor.set(moveArmInput); + } + } + + +// System.out.println(ArmPos); //-6.9 to 1.9 total: 8.8 range + + + //PID encoder position + public double getEncoderArmPosition() + { + return armMotor.getPositionWorld(); + } + + public double getArmHeightInchesAboveFloor() + { + return armMotor.getPositionWorld(); + } + + public synchronized void setControlMode(DriveControlMode controlMode) + { + this.controlMode = controlMode; + + isFinished = false; + } + /* + public void setArmPIDMaxScale(double ArmPosition, double maxError, double minError) + { + double ArmTargetPos = 0; + this.targetPPosition = ArmTargetPos; + pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError); ///////TARGET POSITION WHERE?? + Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_MAX_SCALE); + } + + public void setArmPIDLowScale(double ArmPosition, double maxError, double minError) + { + double ArmTargetPos = 0; + this.targetPPosition = ArmTargetPos; + pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError); + Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_LOW_SCALE); + } + + public void setArmPIDSwitch(double ArmPosition, double maxError, double minError) + { + double ArmTargetPos = 0; + this.targetPPosition = ArmTargetPos; + pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError); + Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_SWITCH); + } + + public void setArmPIDLowest(double ArmPosition, double maxError, double minError) + { + double ArmTargetPos = 0; + this.targetPPosition = ArmTargetPos; + pidPositionControllerMaxScale.setPIDPositionTarget(ArmTargetPos, maxError, minError); + Robot.Arm.setControlMode(DriveControlMode.MOVE_POSITION_LOWEST); + } + */ + public void rawSetOutput(double output){ + armMotor.set(/*ControlMode.PercentOutput,*/ output); + } + + public void holdInPos() + { + armMotor.set(-0.43 * 0.2); + } + + public void stopMotors() + { + armMotor.set(0); + } + + public void isSwitchPressed() + { + pressed = false; + isPressed = armMotor.getSensorCollection(); + + if(isPressed.isFwdLimitSwitchClosed() == true) + { + if (controlMode == DriveControlMode.JOYSTICK) { + Robot.arm.setControlMode(DriveControlMode.STOP_MOTORS); + } + pressed = true; + } + else + { + if (controlMode == DriveControlMode.STOP_MOTORS){ + { + Robot.arm.setControlMode(DriveControlMode.JOYSTICK); + } + + pressed = false; + } + } + + } + + //pressed = (isPressed.isFwdLimitSwitchClosed() == true) ? true : false; + + - - private void controlPidWithJoystick() { - //double joystickPosition = -Robot.oi.getOperatorController().getLeftYAxis(); - //double deltaPosition = joystickPosition *.5; - targetPositionInchesPID = targetPositionInchesPID;// + deltaPosition; - updatePositionPID(targetPositionInchesPID); + @Override + public void controlLoopUpdate() + { + if (controlMode == DriveControlMode.JOYSTICK || controlMode == DriveControlMode.CLIMB) + { + moveArmXbox(); + } + else if (!isFinished) + { + //PID control mode + if(controlMode == DriveControlMode.MOVE_POSITION_MAX_SCALE) + { + isFinished = pidPositionControllerMaxScale.controlLoopUpdate(getEncoderArmPosition()); + } + else if(controlMode == DriveControlMode.MOVE_POSITION_LOW_SCALE) + { + isFinished = pidPositionControllerLowScale.controlLoopUpdate(getEncoderArmPosition()); + } + else if(controlMode == DriveControlMode.MOVE_POSITION_SWITCH) + { + isFinished = pidPositionControllerSwitch.controlLoopUpdate(getEncoderArmPosition()); + } + else if(controlMode == DriveControlMode.MOVE_POSITION_LOWEST) + { + isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderArmPosition()); + } + /* + else if(controlMode == DriveControlMode.RAW) + { + isFinished = pidPositionControllerLowest.controlLoopUpdate(getEncoderArmPosition()); + } + */ + } } - private void controlManualWithJoystick() { - double joyStickSpeed = -Robot.oi.getOperatorController().getLeftYAxis(); - setSpeedJoystick(joyStickSpeed); + @Override + public void initDefaultCommand() + { + } + + @Override + public void periodic() + { + // isSwitchPressed(); + } + + @Override + public void setPeriodMs(long periodMs) + { + //PID controller + pidPositionControllerMaxScale = new SoftwarePIDPositionController(PositionPMaxScale, armMotor); + pidPositionControllerLowScale = new SoftwarePIDPositionController(PositionPLowScale, armMotor); + pidPositionControllerSwitch = new SoftwarePIDPositionController(PositionPSwitch, armMotor); + pidPositionControllerLowest = new SoftwarePIDPositionController(PositionPLowest, armMotor); + + this.periodMs = periodMs; } - - public double getYPositionInches() { - return motor1.getPositionWorld()*Y_POSITION_MATH; - - } - public double getXPositionInches() { - return motor1.getPositionWorld()*X_POSITION_MATH; - } - -// public double getAverageMotorCurrent() { -// return (Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_1_CAN_ID) + Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_2_CAN_ID) + Robot.pdp.getCurrent(RobotMap.ELEVATOR_MOTOR_3_CAN_ID)) / 3; -// } - - public double getAverageMotorCurrent() { - return (motor1.getOutputCurrent() + motor2.getOutputCurrent() / 2); - } - - public synchronized boolean isFinished() { + public synchronized boolean isFinished() + { return isFinished; } - public synchronized void setFinished(boolean isFinished) { - this.isFinished = isFinished; - } - - public double getPeriodMs() { + public double getPeriodMs() + { return periodMs; } - public void updateStatus(Robot.OperationMode operationMode) { - if (operationMode == Robot.OperationMode.TEST) { - try { - SmartDashboard.putNumber("Elevator Position Inches", motor1.getPositionWorld()); - SmartDashboard.putNumber("Elevator Average Amps", getAverageMotorCurrent()); - SmartDashboard.putNumber("Elevator Target PID Position", targetPositionInchesPID); + public void updateStatus(Robot.OperationMode operationMode) + { + if (operationMode == Robot.OperationMode.TEST) + { + try + { + SmartDashboard.putNumber("Arm Pos Ticks", armMotor.getSelectedSensorPosition(0)); + SmartDashboard.putNumber("Arm Pos Inches", getArmHeightInchesAboveFloor()); + //SmartDashboard.putData(pressed); } - catch (Exception e) { + catch (Exception e) + { + System.err.println("Drivetrain update status error"); } } - } - - public static Arm getInstance() { - if(instance == null) { - instance = new Arm(); - } - return instance; } -} + + +} \ No newline at end of file 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 bc9fd7c..5402a51 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 @@ -101,8 +101,6 @@ public class Wrist extends Subsystem private double joystickDegreesPerMs = JOYSTICK_Degrees_PER_MS_LO; LimitSwitchSource limitSwitchSource; - private boolean pressed; - SensorCollection isPressed; public Wrist() {