From: EvanYap Date: Tue, 17 Nov 2015 04:47:00 +0000 (-0800) Subject: Uses arrays and sends message when a specific limitswitch is hit X-Git-Url: http://challenge-bot.com/repos/?a=commitdiff_plain;h=7a25a687d362997c211ff4995f72f339d0a46f50;p=3501%2F2015-FRC-Spark Uses arrays and sends message when a specific limitswitch is hit --- diff --git a/src/org/usfirst/frc3501/RiceCatRobot/commands/LimitSwitchTest.java b/src/org/usfirst/frc3501/RiceCatRobot/commands/LimitSwitchTest.java new file mode 100644 index 0000000..daedb3c --- /dev/null +++ b/src/org/usfirst/frc3501/RiceCatRobot/commands/LimitSwitchTest.java @@ -0,0 +1,15 @@ +package org.usfirst.frc3501.RiceCatRobot.commands; + +import edu.wpi.first.wpilibj.command.CommandGroup; + +public class LimitSwitchTest extends CommandGroup { + /** + * It is necessary for all commands to be used. From MoveArmToLevel1 - MoveArmToLevel3 + */ + + public LimitSwitchTest() { + addSequential( new MoveArmToLevel1() ); + addSequential( new MoveArmToLevel2() ); + addSequential( new MoveArmToLevel3() ); + } +} diff --git a/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveArmToLevel1.java b/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveArmToLevel1.java new file mode 100644 index 0000000..158a4be --- /dev/null +++ b/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveArmToLevel1.java @@ -0,0 +1,33 @@ +package org.usfirst.frc3501.RiceCatRobot.commands; + +import org.usfirst.frc3501.RiceCatRobot.subsystems.Arm; + +import edu.wpi.first.wpilibj.command.Command; + +public class MoveArmToLevel1 extends Command { + Arm arm; + double slowSpeed = 0.2; + + public MoveArmToLevel1() { + arm.setArmSpeeds(slowSpeed); + } + + protected void initialize() { + arm.initializeCounters(); + } + + protected void execute() { + } + + protected boolean isFinished() { + return arm.isSwitch1Hit(); + } + + protected void end() { + System.out.println("Robot arm has reached level 1"); + } + + protected void interrupted() { + end(); + } +} diff --git a/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveArmToLevel2.java b/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveArmToLevel2.java new file mode 100644 index 0000000..4152739 --- /dev/null +++ b/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveArmToLevel2.java @@ -0,0 +1,34 @@ +package org.usfirst.frc3501.RiceCatRobot.commands; + +import org.usfirst.frc3501.RiceCatRobot.subsystems.Arm; + +import edu.wpi.first.wpilibj.command.Command; + +public class MoveArmToLevel2 extends Command { + Arm arm; + double slowSpeed = 0.2; + public MoveArmToLevel2() { + if(arm.getArmSpeed() == 0.0) { + arm.setArmSpeeds(slowSpeed); + } + } + + protected void initialize() { + arm.initializeCounters(); + } + + protected void execute() { + } + + protected boolean isFinished() { + return arm.isSwitch2Hit(); + } + + protected void end() { + System.out.println("Robot arm has reached level 2"); + } + + protected void interrupted() { + end(); + } +} diff --git a/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveArmToLevel3.java b/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveArmToLevel3.java new file mode 100644 index 0000000..d969dee --- /dev/null +++ b/src/org/usfirst/frc3501/RiceCatRobot/commands/MoveArmToLevel3.java @@ -0,0 +1,37 @@ +package org.usfirst.frc3501.RiceCatRobot.commands; + +import org.usfirst.frc3501.RiceCatRobot.subsystems.Arm; + +import edu.wpi.first.wpilibj.command.Command; + +public class MoveArmToLevel3 extends Command { + Arm arm; + + double slowSpeed = 0.2; + + public MoveArmToLevel3() { + if (arm.getArmSpeed() == 0.0) { + arm.setArmSpeeds(slowSpeed); + } + } + + protected void initialize() { + arm.initializeCounters(); + } + + protected void execute() { + } + + protected boolean isFinished() { + return arm.isSwitch3Hit(); + } + + protected void end() { + System.out.println("Robot arm has reached level 3"); + arm.stop(); + } + + protected void interrupted() { + end(); + } +} diff --git a/src/org/usfirst/frc3501/RiceCatRobot/subsystems/Arm.java b/src/org/usfirst/frc3501/RiceCatRobot/subsystems/Arm.java index 8d78c8f..a29262a 100644 --- a/src/org/usfirst/frc3501/RiceCatRobot/subsystems/Arm.java +++ b/src/org/usfirst/frc3501/RiceCatRobot/subsystems/Arm.java @@ -3,45 +3,87 @@ package org.usfirst.frc3501.RiceCatRobot.subsystems; import org.usfirst.frc3501.RiceCatRobot.RobotMap; import edu.wpi.first.wpilibj.CANJaguar; +import edu.wpi.first.wpilibj.Counter; +import edu.wpi.first.wpilibj.DigitalInput; import edu.wpi.first.wpilibj.command.Subsystem; public class Arm extends Subsystem { - private CANJaguar left, right; + private CANJaguar left, right; + double speedOfArm = 0.0; - public Arm() { - left = new CANJaguar(RobotMap.ARM_LEFT); - right = new CANJaguar(RobotMap.ARM_RIGHT); - } + DigitalInput[] limitSwitches = new DigitalInput[3]; + Counter[] counters = new Counter[3]; - public void initDefaultCommand() { - } + public Arm() { + left = new CANJaguar(RobotMap.ARM_LEFT); + right = new CANJaguar(RobotMap.ARM_RIGHT); + } - public void fineTuneControl(double d) { - if (Math.abs(d) < 0.05) { - d = 0; - } else if (d > 0) { - d *= d; - } else { - d *= -d; - } - setArmSpeeds(d); - } + public void setDigitalInputValues() { + limitSwitches[0] = new DigitalInput(0); + limitSwitches[1] = new DigitalInput(1); + limitSwitches[2] = new DigitalInput(2); + } - public void setLeft(double speed) { - left.set(-speed); - } + public void setCountersCorrespondingToSwitches() { + counters[0] = new Counter(0); + counters[1] = new Counter(1); + counters[3] = new Counter(2); + } - public void setRight(double speed) { - right.set(-speed); + public double getArmSpeed() { + return this.speedOfArm; } - public void setArmSpeeds(double speed) { - setLeft(speed); - setRight(speed); - } + public boolean isSwitch1Hit() { + return counters[0].get() > 0; + } - public void stop() { - left.set(0); - right.set(0); - } + public boolean isSwitch2Hit() { + return counters[1].get() > 0; + } + + public boolean isSwitch3Hit() { + return counters[2].get() > 0; + } + + public void initializeCounters() { + for (int i = 0; i < counters.length; i++) { + counters[i].reset(); + } + } + + public void initDefaultCommand() { + } + + public void fineTuneControl(double d) { + if (Math.abs(d) < 0.05) { + d = 0; + } else if (d > 0) { + d *= d; + } else { + d *= -d; + } + setArmSpeeds(d); + } + + public void setLeft(double speed) { + left.set(-speed); + } + + public void setRight(double speed) { + right.set(-speed); + } + + public void setArmSpeeds(double speed) { + this.speedOfArm = speed; + setLeft(speed); + setRight(speed); + } + + public void stop() { + this.speedOfArm = 0.0; + left.set(0); + right.set(0); + } }