team2059/ford-java
2
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

limit switch attemp 1

Browse Source
This commit is contained in:
ian 2016-09-16 20:04:15 -04:00
parent 3b1efebba8
commit 16db69d087
2 changed files with 15 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/org/usfirst/frc/team2059/robot/RobotMap.java
View File

@ -10,6 +10,8 @@ public class RobotMap {
//Arm //Arm
public static double zeroDegrees = 2.019; public static double zeroDegrees = 2.019;
public static double ninetyDegrees = 3.512; public static double ninetyDegrees = 3.512;
public static int armBottomLim = 4;
public static int armTopLim = 5;
public static int armPot = 0; public static int armPot = 0;
public static int armLeftMotor = 5; public static int armLeftMotor = 5;
public static int armRightMotor = 6; public static int armRightMotor = 6;

13
src/org/usfirst/frc/team2059/robot/subsystems/MainArm.java
View File

@ -3,11 +3,15 @@ import org.usfirst.frc.team2059.robot.RobotMap;
import org.usfirst.frc.team2059.robot.commands.Drive; import org.usfirst.frc.team2059.robot.commands.Drive;
import edu.wpi.first.wpilibj.command.PIDSubsystem; import edu.wpi.first.wpilibj.command.PIDSubsystem;
import edu.wpi.first.wpilibj.CANTalon; import edu.wpi.first.wpilibj.CANTalon;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.AnalogInput; import edu.wpi.first.wpilibj.AnalogInput;
public class MainArm extends PIDSubsystem { public class MainArm extends PIDSubsystem {
CANTalon armMotorLeft = new CANTalon(RobotMap.armLeftMotor); CANTalon armMotorLeft = new CANTalon(RobotMap.armLeftMotor);
CANTalon armMotorRight = new CANTalon(RobotMap.armRightMotor); CANTalon armMotorRight = new CANTalon(RobotMap.armRightMotor);
AnalogInput pot = new AnalogInput(RobotMap.armPot); AnalogInput pot = new AnalogInput(RobotMap.armPot);
DigitalInput limitSwitchBottom = new DigitalInput(RobotMap.armBottomLim);
DigitalInput limitSwitchTop = new DigitalInput(RobotMap.armTopLim);
public MainArm() { public MainArm() {
super("MainArm", 0.06, 0.0, 0.002); super("MainArm", 0.06, 0.0, 0.002);
getPIDController().setContinuous(false); getPIDController().setContinuous(false);
@ -34,10 +38,19 @@ public class MainArm extends PIDSubsystem {
return potToDegrees(getRaw()); return potToDegrees(getRaw());
} }
private double potToDegrees(double pot) { private double potToDegrees(double pot) {
if (limitSwitchBottom.get()){
RobotMap.zeroDegrees = getRaw();
System.out.println("====Bottom Switch====\n" + getRaw());
} else if (limitSwitchTop.get()){
RobotMap.ninetyDegrees = getRaw();
System.out.println("====Top Switch====\n" + getRaw());
}
double min = RobotMap.zeroDegrees; double min = RobotMap.zeroDegrees;
double max = RobotMap.ninetyDegrees; double max = RobotMap.ninetyDegrees;
System.out.println((pot - min) / (Math.abs(min - max) / 90)); System.out.println((pot - min) / (Math.abs(min - max) / 90));
return (pot - min) / (Math.abs(min - max) / 90); return (pot - min) / (Math.abs(min - max) / 90);
} }
} }
// vim: sw=2:ts=2:sts=2 // vim: sw=2:ts=2:sts=2