package org.robockets.stronghold.robot;

import com.kauailabs.nav6.frc.IMUAdvanced;

import edu.wpi.first.wpilibj.CANJaguar;
import edu.wpi.first.wpilibj.CANTalon;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.SerialPort;
import edu.wpi.first.wpilibj.Victor;

/**
 * The RobotMap is a mapping from the ports sensors and actuators are wired into
 * to a variable name. This provides flexibility changing wiring, makes checking
 * the wiring easier and significantly reduces the number of magic numbers
 * floating around.
 */
public class RobotMap {
	private static SerialPort navXSerialPort = new SerialPort(57600, SerialPort.Port.kMXP);
	private static byte updateRateHz = 50;
	public static IMUAdvanced navX = new IMUAdvanced(navXSerialPort, updateRateHz);
	public static RobotDrive robotDrive = new RobotDrive(0, 1);
	public static Victor intakeRollerMotor = new Victor(2); //TEMP
	public static Encoder intakeMotorEncoder = new Encoder(3, 4);
	public static Victor intakeVerticalMotor = new Victor(3); //TEMP
	public static Victor jeffRoller1 = new Victor(4); // TEMP
	public static Victor jeffRoller2 = new Victor(5); // TEMP
	public static Victor turnTableMotor = new Victor(9); // TEMP
	public static Encoder turnTableEncoder = new Encoder(5, 6);
	public static Victor hoodMotor = new Victor(8); // TEMP
	public static Encoder hoodEncoder = new Encoder(7, 8);
	public static Encoder driveEncoder = new Encoder(9, 10);
	public static CANTalon shootingWheelMotor = new CANTalon(0); //TODO: Figure out the id of the motor over CAN.

	
	public RobotMap () {
		navX.zeroYaw();
		turnTableEncoder.reset();
		shootingWheelMotor.setFeedbackDevice(CANTalon.FeedbackDevice.QuadEncoder);
		shootingWheelMotor.changeControlMode(CANTalon.TalonControlMode.Speed);
		shootingWheelMotor.setPID(0.1, 0, 0);
		shootingWheelMotor.enableControl();
		shootingWheelMotor.enable();
	}
}

@@ -82,48 +82,8 @@
 d;%0A%0A
-import edu.wpi.first.wpilibj.CANJaguar;%0A
 impo
@@ -1196,17 +1196,17 @@
  Victor(
-9
+7
 ); // TE
@@ -1379,12 +1379,12 @@
 der(
-7, 8
+0, 1
 );%0A%09
@@ -1502,60 +1502,11 @@
 lon(
-0
+2
 );
- //TODO: Figure out the id of the motor over CAN.
 %0A%0A%09%0A
@@ -1549,36 +1549,8 @@
 ();%0A
-%09%09turnTableEncoder.reset();%0A
 %09%09sh
@@ -1612,19 +1612,31 @@
 ice.
-QuadEncoder
+CtreMagEncoder_Relative
 );%0A%09
@@ -1660,165 +1660,94 @@
 or.c
-hangeControlMode(CANTalon.TalonControlMode.Speed);%0A%09%09shootingWheelMotor.setPID(0.1, 0, 0);%0A%09%09shootingWheelMotor.enableControl();%0A%09%09shootingWheelMotor.enable(
+onfigEncoderCodesPerRev(1);%0A%09%09turnTableEncoder.setDistancePerPulse(1/(7*71*180/40/360)
 );%0A%09

Apply turn table gear ratio.