// Path: Robot/src/main/java/org/usfirst/frc/team2557/robot/RobotMap.java
// public class RobotMap {
// 	
// 
//     public static CANTalon driveLeft1;
//     public static CANTalon driveLeft2;
//     public static CANTalon driveRight1;
//     public static CANTalon driveRight2;
//     public static CANTalon catapultMotor;
//     public static CANTalon climbingMotor;
//     public static CANTalon leftActuatorMotor;
//     public static CANTalon rightActuatorMotor;
//     
//     public static CANTalon intakeMotor;
// 
//     public static Servo secondaryArm;
// 
//     public static DigitalInput catapultHallEffect;
// 
//     public static Accelerometer rioAccelerometer;
// 
//     public static Gyro mainGyro;
// 
//     public static AnalogInput leftPotentiometer;
//     public static AnalogInput rightPotentiometer;
// 
//     public static LidarRangeFinder lidarSensor;
//     public static AnalogInput sonar;
// 
//     public static ArduinoComm arduinoComm;
// 
//     public static RobotDrive robotDrive;
// 
//     /**
//      * Initializes all the parts of RobotMap. This must be called main init method
//      * in Robot.java, as WPI needs to set up before these objects can be constructed.
//      */
//     public static void init() {
//     	
//     	/*
//     	 * CAN ports 6, 8, 2, and 3 have encoder breakouts.
//     	 */
//         driveLeft1 = new CANTalon(7);
//         driveLeft2 = new CANTalon(6);
//         driveRight1 = new CANTalon(8);
//         driveRight2 = new CANTalon(5);
//         catapultMotor = new CANTalon(1);
//         climbingMotor = new CANTalon(9);
//         intakeMotor = new CANTalon(4);
// 
//         leftActuatorMotor = new CANTalon(2);
//         leftActuatorMotor.enableLimitSwitch(false, false);
//         leftActuatorMotor.enableBrakeMode(true);
//         rightActuatorMotor = new CANTalon(3);
//         rightActuatorMotor.enableLimitSwitch(false, false);
//         rightActuatorMotor.enableBrakeMode(true);
// 
//         secondaryArm = new Servo(4);
// 
//         catapultHallEffect = new DigitalInput(0);
// 
//         rioAccelerometer = new BuiltInAccelerometer();
// 
//         mainGyro = new ADXRS450_Gyro();
// 
//         leftPotentiometer = new AnalogInput(1);
//         rightPotentiometer = new AnalogInput(2);
// 
//         lidarSensor = new LidarRangeFinder(SerialPort.Port.kMXP); // Using the MXP breakout for tx/rx (serial)
// 
//         robotDrive = new RobotDrive(driveLeft1, driveLeft2, driveRight1, driveRight2);
//         robotDrive.setExpiration(0.2);
//         robotDrive.setSafetyEnabled(false);
// 
//         arduinoComm = new ArduinoComm(); // Using the MXP breakout for sda/sdt (i2c)
//     }
// 
// }
// 
// Path: Robot/src/main/java/org/usfirst/frc/team2557/robot/commands/chassis/DriveCommand.java
// public class DriveCommand extends Command {
// 
//     public DriveCommand() {
//         requires(Robot.chassis);
//     }
// 
//     // Called just before this Command runs the first time
//     protected void initialize() {
//     }
// 
//     // Called repeatedly when this Command is scheduled to run
//     protected void execute() {
//         double speedEdit = 1 - (Robot.oi.driver.getRawAxis(3) + 1) / 2;
// 
//         // Calculate arcade drive so we can use our custom "set" method
//         double power = -Robot.oi.driver.getRawAxis(1);
//         double turn = -Robot.oi.driver.getRawAxis(0);
//         Robot.chassis.set((power - turn) * speedEdit,
//                 (power + turn) * speedEdit);
// 
//     }
// 
//     // Make this return true when this Command no longer needs to run execute()
//     protected boolean isFinished() {
//         return false;
//     }
// 
//     // Called once after isFinished returns true
//     protected void end() {
// 
//     }
// 
//     // Called when another command which requires one or more of the same
//     // subsystems is scheduled to run
//     protected void interrupted() {
//         this.end();
//     }
// }


import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.interfaces.Gyro;
import org.usfirst.frc.team2557.robot.RobotMap;
import edu.wpi.first.wpilibj.command.Subsystem;
import org.usfirst.frc.team2557.robot.commands.chassis.DriveCommand;
package org.usfirst.frc.team2557.robot.subsystems;



public class Chassis extends Subsystem {

//    CANTalon leftEncTalon = RobotMap.driveLeft2;
//    CANTalon rightEncTalon = RobotMap.driveRight1;
    // TODO: See whether the comp bot's encoders are switched as well
    CANTalon leftEncTalon = RobotMap.driveRight1; // Right and left are switched on the practice bot
    CANTalon rightEncTalon = RobotMap.driveLeft2;

    RobotDrive drive = RobotMap.robotDrive;
    Gyro gyro = RobotMap.mainGyro;

    double leftPosResetValue = 0;
    double rightPosResetValue = 0;

    private final double posToMeters = (14 / 28) * 8 * 3.14159;
    
    public Chassis() {
    	
    }

    public void initDefaultCommand() {
// Path: Robot/src/main/java/org/usfirst/frc/team2557/robot/RobotMap.java
// public class RobotMap {
// 	
// 
//     public static CANTalon driveLeft1;
//     public static CANTalon driveLeft2;
//     public static CANTalon driveRight1;
//     public static CANTalon driveRight2;
//     public static CANTalon catapultMotor;
//     public static CANTalon climbingMotor;
//     public static CANTalon leftActuatorMotor;
//     public static CANTalon rightActuatorMotor;
//     
//     public static CANTalon intakeMotor;
// 
//     public static Servo secondaryArm;
// 
//     public static DigitalInput catapultHallEffect;
// 
//     public static Accelerometer rioAccelerometer;
// 
//     public static Gyro mainGyro;
// 
//     public static AnalogInput leftPotentiometer;
//     public static AnalogInput rightPotentiometer;
// 
//     public static LidarRangeFinder lidarSensor;
//     public static AnalogInput sonar;
// 
//     public static ArduinoComm arduinoComm;
// 
//     public static RobotDrive robotDrive;
// 
//     /**
//      * Initializes all the parts of RobotMap. This must be called main init method
//      * in Robot.java, as WPI needs to set up before these objects can be constructed.
//      */
//     public static void init() {
//     	
//     	/*
//     	 * CAN ports 6, 8, 2, and 3 have encoder breakouts.
//     	 */
//         driveLeft1 = new CANTalon(7);
//         driveLeft2 = new CANTalon(6);
//         driveRight1 = new CANTalon(8);
//         driveRight2 = new CANTalon(5);
//         catapultMotor = new CANTalon(1);
//         climbingMotor = new CANTalon(9);
//         intakeMotor = new CANTalon(4);
// 
//         leftActuatorMotor = new CANTalon(2);
//         leftActuatorMotor.enableLimitSwitch(false, false);
//         leftActuatorMotor.enableBrakeMode(true);
//         rightActuatorMotor = new CANTalon(3);
//         rightActuatorMotor.enableLimitSwitch(false, false);
//         rightActuatorMotor.enableBrakeMode(true);
// 
//         secondaryArm = new Servo(4);
// 
//         catapultHallEffect = new DigitalInput(0);
// 
//         rioAccelerometer = new BuiltInAccelerometer();
// 
//         mainGyro = new ADXRS450_Gyro();
// 
//         leftPotentiometer = new AnalogInput(1);
//         rightPotentiometer = new AnalogInput(2);
// 
//         lidarSensor = new LidarRangeFinder(SerialPort.Port.kMXP); // Using the MXP breakout for tx/rx (serial)
// 
//         robotDrive = new RobotDrive(driveLeft1, driveLeft2, driveRight1, driveRight2);
//         robotDrive.setExpiration(0.2);
//         robotDrive.setSafetyEnabled(false);
// 
//         arduinoComm = new ArduinoComm(); // Using the MXP breakout for sda/sdt (i2c)
//     }
// 
// }
// 
// Path: Robot/src/main/java/org/usfirst/frc/team2557/robot/commands/chassis/DriveCommand.java
// public class DriveCommand extends Command {
// 
//     public DriveCommand() {
//         requires(Robot.chassis);
//     }
// 
//     // Called just before this Command runs the first time
//     protected void initialize() {
//     }
// 
//     // Called repeatedly when this Command is scheduled to run
//     protected void execute() {
//         double speedEdit = 1 - (Robot.oi.driver.getRawAxis(3) + 1) / 2;
// 
//         // Calculate arcade drive so we can use our custom "set" method
//         double power = -Robot.oi.driver.getRawAxis(1);
//         double turn = -Robot.oi.driver.getRawAxis(0);
//         Robot.chassis.set((power - turn) * speedEdit,
//                 (power + turn) * speedEdit);
// 
//     }
// 
//     // Make this return true when this Command no longer needs to run execute()
//     protected boolean isFinished() {
//         return false;
//     }
// 
//     // Called once after isFinished returns true
//     protected void end() {
// 
//     }
// 
//     // Called when another command which requires one or more of the same
//     // subsystems is scheduled to run
//     protected void interrupted() {
//         this.end();
//     }
// }


// Path: Robot/src/main/java/org/usfirst/frc/team2557/robot/subsystems/Chassis.java
import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.interfaces.Gyro;
import org.usfirst.frc.team2557.robot.RobotMap;
import edu.wpi.first.wpilibj.command.Subsystem;
import org.usfirst.frc.team2557.robot.commands.chassis.DriveCommand;

package org.usfirst.frc.team2557.robot.subsystems;



public class Chassis extends Subsystem {

//    CANTalon leftEncTalon = RobotMap.driveLeft2;
//    CANTalon rightEncTalon = RobotMap.driveRight1;
    // TODO: See whether the comp bot's encoders are switched as well
    CANTalon leftEncTalon = RobotMap.driveRight1; // Right and left are switched on the practice bot
    CANTalon rightEncTalon = RobotMap.driveLeft2;

    RobotDrive drive = RobotMap.robotDrive;
    Gyro gyro = RobotMap.mainGyro;

    double leftPosResetValue = 0;
    double rightPosResetValue = 0;

    private final double posToMeters = (14 / 28) * 8 * 3.14159;
    
    public Chassis() {
    	
    }

    public void initDefaultCommand() {
        setDefaultCommand(new DriveCommand());