
package org.usfirst.frc.team2339.Barracuda;

import org.usfirst.frc.team2339.Barracuda.RobotMap.SwerveMap;
import org.usfirst.frc.team2339.Barracuda.subsystems.SwerveDrive;
import org.usfirst.frc.team2339.Barracuda.subsystems.SwerveDriveRectangle;

import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.Talon;


/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Barracuda extends IterativeRobot {
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    /**********************
     * CLASS VARIABLES
    /**********************/
    

    private SwerveDriveRectangle robotDrive = null; 
    private final Talon winchMotor = new Talon(RobotMap.WinchMap.LIFT_WINCH);
      
   
    /*
     * Vision class
    */
   //* private final RecycleRushV2ision visionControl = new RecycleRushVision();

    
    /*
     * Initialize values for autonomous control
     */
    private long startTime = 0;
    
    /*
     * Time variables to help with timed printouts
     */
    private long robotStartTime = 0;
    
    /**********************
     * CLASS METHODS
     * Methods for this class are below here
    /**********************/
    
    /*
     * This method sets winch motors
     * 
     * @param value motor speed
     */
    public void setWinchMotor(double value) {
        SmartDashboard.putNumber("Winch motor value ", value);
        winchMotor.set(value);
    }
    
    /**
     * This method is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
        robotStartTime = System.currentTimeMillis();
        System.out.println("Robot init time: " + robotStartTime);
        
        robotDrive = new SwerveDriveRectangle(SwerveDriveRectangle.createWheels(
        		SwerveMap.Constants.WHEEL_BASE_LENGTH, 
        		SwerveMap.Constants.WHEEL_BASE_WIDTH, 
        		SwerveMap.Wheel.DRIVE_CONTROLLERS, 
        		SwerveMap.Wheel.STEERING_PID_CONTROLLERS));
        
        robotDrive.resetSteering();
        
       //* visionControl.visionInit();
        System.out.println("End robot init: " + System.currentTimeMillis());
    }
    

    /**
     * This method is called at the beginning of autonomous period
     */
	public void autonomousPeriodicInit() {
		startTime = System.currentTimeMillis();
		System.out.println("Autonomous init time: " + startTime);
       	RobotMap.Control.GYRO.reset();
        robotDrive.resetSteering();
        robotDrive.enableSteering(true);
    }
	
    public void autonomousPeriodic() {
    }
    
    /**
     * This method is called at the beginning of operator control
     */
    public void teleopInit() {
    	RobotMap.Control.GYRO.reset();
        robotDrive.resetSteering();
        robotDrive.enableSteering(true);
    }

 

    /**
     * This function is called periodically during operator control
     */

    public void teleopPeriodic() {
   
		/*
		 * Print out siginigicant changes in drive info
		 */
		SmartDashboard.putNumber("Joystick forward ", RobotMap.Control.DRIVE_STICK.getRawAxis(SwerveMap.Control.DRIVE_AXIS_FORWARD_BACK));
		SmartDashboard.putNumber("Joystick sideways ", RobotMap.Control.DRIVE_STICK.getRawAxis(SwerveMap.Control.DRIVE_AXIS_SIDEWAYS));
		SmartDashboard.putNumber("Joystick rotate ", RobotMap.Control.DRIVE_STICK.getRawAxis(SwerveMap.Control.DRIVE_AXIS_ROTATE));
		SmartDashboard.putNumber("Gyro angle ", RobotMap.Control.GYRO.getAngle());
          
    	if(RobotMap.Control.OPERATOR_STICK.getRawButton(RobotMap.Control.GYRO_BUTTON_RESET)) {
    		// Reset gyro
    		RobotMap.Control.GYRO.reset();
    	}
    	
        /*
         * Set winch motors
         */
    	setWinchMotor(RobotMap.WinchMap.WINCH_STICK.getRawAxis(RobotMap.WinchMap.WINCH_AXIS));
    	  
        
		/*
		 * Drive robot based on values from joystick
		 */
		robotDrive.swerveDriveTeleop();
    	
    	// Test a wheel
    	/*
    	robotDrive.setWheelPod(SwerveDrive.frontLeft, 180 * RobotMap.Control.DRIVE_STICK.getRawAxis(SwerveMap.Control.DRIVE_AXIS_ROTATE), 
    			RobotMap.Control.DRIVE_STICK.getRawAxis(RobotMap.SwerveMap.Control.DRIVE_AXIS_FORWARD_BACK));
    	robotDrive.setWheelPod(SwerveDrive.frontRight, 180 * RobotMap.Control.DRIVE_STICK.getRawAxis(SwerveMap.Control.DRIVE_AXIS_ROTATE), 
    			RobotMap.Control.DRIVE_STICK.getRawAxis(RobotMap.SwerveMap.Control.DRIVE_AXIS_FORWARD_BACK));
    	robotDrive.setWheelPod(SwerveDrive.rearLeft, 180 * RobotMap.Control.DRIVE_STICK.getRawAxis(SwerveMap.Control.DRIVE_AXIS_ROTATE), 
    			RobotMap.Control.DRIVE_STICK.getRawAxis(RobotMap.SwerveMap.Control.DRIVE_AXIS_FORWARD_BACK));
    	robotDrive.setWheelPod(SwerveDrive.rearRight, 180 * RobotMap.Control.DRIVE_STICK.getRawAxis(SwerveMap.Control.DRIVE_AXIS_ROTATE), 
    			RobotMap.Control.DRIVE_STICK.getRawAxis(SwerveMap.Control.DRIVE_AXIS_FORWARD_BACK));
    			*/
          
    }
 

	/**
     * This function is called periodically during test mode
     */
    public void testPeriodic() {
        long time = System.currentTimeMillis();
        SmartDashboard.putNumber("Test Mode Running Time ", time);
       // long elapsed = System.currentTimeMillis() - startTime;
    }
    
}

@@ -101,16 +101,79 @@
 rveMap;%0A
+import org.usfirst.frc.team2339.Barracuda.commands.TimedDrive;%0A
 import o
@@ -351,16 +351,67 @@
 eRobot;%0A
+import edu.wpi.first.wpilibj.command.CommandGroup;%0A
 import e
@@ -3027,24 +3027,342 @@
 ring(true);%0A
+        %0A        CommandGroup autoCommands = new CommandGroup(%22Autonomous Commands%22);%0A        autoCommands.addSequential(new TimedDrive(%22Push RC to wall%22, robotDrive, 1.0, 0.5, 0.0));%0A        autoCommands.addSequential(new TimedDrive(%22Backup to auto zone%22, robotDrive, 2.0, 0.5, 180.0));%0A        autoCommands.start();%0A
     %7D%0A%09%0A    

Add drive commands to autonomous.