Adding Camera servo code. Needs to be transfered to a command/subsystem

FIRST-Team-2557-The-SOTABots · Mar 5, 2017 · 8d0b71d · 8d0b71d
1 parent a17e764
commit 8d0b71d
Show file tree

Hide file tree

Showing 3 changed files with 16 additions and 2 deletions.
diff --git a/FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/Robot.java b/FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/Robot.java
@@ -46,7 +46,7 @@ public class Robot extends IterativeRobot {
 	public static final Acceleration_sub 	accel		= new Acceleration_sub();
 	public static final SmartDashboard_sub  dashboard 	= new SmartDashboard_sub();
 	public static OI oi;
-
+	public double x = 45;
 
 
 	Command _baseline;
@@ -144,7 +144,16 @@ public void teleopInit() {
 	@Override
 	public void teleopPeriodic() {
 		Scheduler.getInstance().run();
-
+		if(Robot.oi.gamepad2.getPOV() == 270){
+			x += 1;
+		}
+		else if(Robot.oi.gamepad2.getPOV() == 90){
+			x -=1;
+		}
+		else{		
+			x += 0;
+		}
+		RobotMap.cameraServo.setAngle(x);
 	}
 
 	/**

diff --git a/FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/RobotMap.java b/FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/RobotMap.java
@@ -57,6 +57,7 @@ public class RobotMap {
 
 	public static Servo leftAgitator;
 	public static Servo rightAgitator;
+	public static Servo cameraServo;
 
 	public static void init(){
 		FRdrive = new CANTalon(1);
@@ -79,6 +80,7 @@ public static void init(){
 		gearGrab = new DoubleSolenoid(2,3);
 		leftAgitator = new Servo(1);
 		rightAgitator = new Servo(2);
+		cameraServo = new Servo(3);
 
 
 		navX = new AHRS(SPI.Port.kMXP);

diff --git a/FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/subsystems/SmartDashboard_sub.java b/FRC_Robot2.0/src/org/usfirst/frc/team2557/robot/subsystems/SmartDashboard_sub.java
@@ -32,6 +32,9 @@ public void dashboard(){
 //		SmartDashboard.putNumber("Left Servo: ", RobotMap.leftAgitator.getAngle());
 //		SmartDashboard.putNumber("Right Servo: ", RobotMap.rightAgitator.getAngle());
 //		SmartDashboard.putBoolean("Gear Switch is: ", RobotMap.gearSwitch.get());
+    	SmartDashboard.putNumber("", RobotMap.cameraServo.getAngle());
+
+
     	if(RobotMap.gearGrab.get() == Value.kForward){
     		SmartDashboard.putBoolean("Is closed/open", true);
     	}