[SandySandySand]

Hello there, I am a first year programmer with my team and we are working in Java this year, for the first time.(We are using Command-Based, instead of going for the Iterative Robot API.) We have recently looked into using a gyro for this year's autonomous mode and are having trouble. We are able to get a reading of the angle, but no matter what we try we can not modify the motor speed using this newly found angle we have. I have included the code we are using. We have tried 2 different methods to fix this problem. (The first method the code that we have commented out.) Any help is appreciated. Thank you, friends.

 protected void execute() {
    	//double heading = Robot.gyro.getRawAngle();
        //double motorPower = getSafeTan(heading);
        //Robot.drivetrain.arcadeDrive(0, motorPower);
    	double correction = 0.03;
    	double a =Robot.gyro.getRawAngle()*correction;
    	Robot.drivetrain.drive(0, a);
    }

public double getSafeTan(double heading)
    {
    	double safeAngle = heading / 45.0;
    	//now safeAngle can be NaN or -56 or some crazy useless number. 
    	//We have elected to just attempt basic corrections in that case.
    	if(safeAngle > 1)
    	{
    		safeAngle = 1;
    	}
    	
    	if(safeAngle < -1)
    	{
    		safeAngle = -1;
    	}
    	
    	if(safeAngle > 1 || safeAngle < -1)
    	{
    		safeAngle = 0;
    	}
    	
    	return safeAngle;
    }