/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package com.edinarobotics.zed;


import com.edinarobotics.utils.commands.MaintainStateCommand;
import com.edinarobotics.utils.gamepad.Gamepad;
import com.edinarobotics.zed.commands.GamepadDriveStrafeCommand;
import com.edinarobotics.zed.commands.GamepadDriveRotationCommand;
import com.edinarobotics.zed.subsystems.Drivetrain;
import com.edinarobotics.zed.subsystems.DrivetrainRotation;
import com.edinarobotics.zed.subsystems.DrivetrainStrafe;
import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.command.Scheduler;

public class Zed extends IterativeRobot {
    
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    public void robotInit() {
        Components.getInstance(); //Create all robot subsystems.
    }
    
    /**
     * This function is called once at the start of autonomous mode.
     */
    public void autonomousInit(){
        DrivetrainStrafe drivetrainStrafe = Components.getInstance().drivetrainStrafe;
        drivetrainStrafe.setDefaultCommand(new MaintainStateCommand(drivetrainStrafe));
        DrivetrainRotation drivetrainRotation = Components.getInstance().drivetrainRotation;
        drivetrainRotation.setDefaultCommand(new MaintainStateCommand(drivetrainRotation));
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
        Scheduler.getInstance().run();
    }
    
    /**
     * This function is called once at the start of teleop mode.
     */
    public void teleopInit(){
        DrivetrainStrafe drivetrainStrafe = Components.getInstance().drivetrainStrafe;
        DrivetrainRotation drivetrainRotation = Components.getInstance().drivetrainRotation;
        Gamepad driveGamepad = Controls.getInstance().gamepad1;
        
        drivetrainStrafe.setDefaultCommand(new GamepadDriveStrafeCommand(driveGamepad, drivetrainStrafe));
        drivetrainRotation.setDefaultCommand(new GamepadDriveRotationCommand(driveGamepad, drivetrainRotation));
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        Scheduler.getInstance().run();
    }
    
    /**
     * This function is called once at the start of test mode.
     */
    public void testInit(){
        DrivetrainStrafe drivetrainStrafe = Components.getInstance().drivetrainStrafe;
        DrivetrainRotation drivetrainRotation = Components.getInstance().drivetrainRotation;
        Gamepad driveGamepad = Controls.getInstance().gamepad1;
        
        drivetrainStrafe.setDefaultCommand(new GamepadDriveStrafeCommand(driveGamepad, drivetrainStrafe));
        drivetrainRotation.setDefaultCommand(new GamepadDriveRotationCommand(driveGamepad, drivetrainRotation));
    }
    
    /**
     * This function is called periodically during test mode
     */
    public void testPeriodic() {
        Scheduler.getInstance().run();
    }
    
}

@@ -623,16 +623,69 @@
 amepad;%0A
+import com.edinarobotics.utils.pid.PIDTuningManager;%0A
 import c
@@ -3504,29 +3504,81 @@
 stance().run();%0A
+        PIDTuningManager.getInstance().runTuning();%0A
     %7D%0A    %0A%7D%0A

Add PID tuning code to Zed.