package org.usfirst.frc.team4828;

import edu.wpi.first.wpilibj.IterativeRobot;
import edu.wpi.first.wpilibj.Joystick;
import com.kauailabs.navx.frc.AHRS;
import edu.wpi.first.wpilibj.SPI;

public class Robot extends IterativeRobot {
    Joystick driveStick;
    DriveTrain drive;
    static AHRS navx;

    @Override
    public void robotInit() {
        super.robotInit();
        System.out.println("THE ROBOT TURNED ON");
        driveStick = new Joystick(0);
        drive = new DriveTrain(1, 2, 3, 4);
        navx = new AHRS(SPI.Port.kMXP);
    }

    @Override
    public void autonomousInit() {
        super.autonomousInit();
        System.out.println("Entering auton...");
    }

    @Override
    public void autonomousPeriodic() {
        super.autonomousPeriodic();
    }

    @Override
    public void teleopInit() {
        super.teleopInit();
        System.out.println("Entering teleop...");
    }

    @Override
    public void teleopPeriodic() {
        super.teleopPeriodic();
        drive.mecanumDrive(driveStick.getX()/2,driveStick.getY()/2,driveStick.getTwist()/8, navx.getAngle());
    }

    @Override
    public void testInit() {
        super.testInit();
        System.out.println("Entering test...");
    }

    @Override
    public void testPeriodic(){
        drive.testMotors();
    }
}

@@ -1085,10 +1085,144 @@
 t()/
-8,
+2, navx.getAngle());%0A        if(driveStick.getRawButton(11))%7B%0A            navx.reset();%0A        %7D%0A        System.out.println(%22Angle: %22 +
  nav
@@ -1425,25 +1425,54 @@
     
-drive.testMotors(
+System.out.println(%22Angle: %22 + navx.getAngle()
 );%0A 

Add ability to zero gyro