<commit_before>package org.usfirst.frc2832.Robot_2016.commands.autonomous;

import org.usfirst.frc2832.Robot_2016.RobotMap;
import org.usfirst.frc2832.Robot_2016.TrajectoryController;

import edu.wpi.first.wpilibj.command.Command;

/**
 *
 */
public class RotateAngle extends Command {
	static double angle, initVal;
	static final double TOLERANCE = 2;
	static TrajectoryController tc;
	
	//IN DEGREES
    public RotateAngle(double theta) {
        // Use requires() here to declare subsystem dependencies
        // e.g. requires(chassis);
    	angle = theta;
    }

    // Called just before this Command runs the first time
    protected void initialize() {
    	initVal = RobotMap.imu.getYaw();
    	tc = new TrajectoryController(angle, 0.3, 0.4, 0.5, 1, -1); //TO-DO: would be nice to test these numbers!
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    	RobotMap.driveTrain.arcadeDrive(0, tc.get(RobotMap.imu.getYaw())); 
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return Math.abs(RobotMap.imu.getYaw() - initVal - angle) < TOLERANCE;
    }

    // Called once after isFinished returns true
    protected void end() {
    	RobotMap.driveTrain.arcadeDrive(0, 0);
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    }<commit_msg>Fix RotateAngle.java to account for gyro wraparound (it jumps from -180 to 180 and vice versa).<commit_after>package org.usfirst.frc2832.Robot_2016.commands.autonomous;

import org.usfirst.frc2832.Robot_2016.RobotMap;
import org.usfirst.frc2832.Robot_2016.TrajectoryController;

import edu.wpi.first.wpilibj.command.Command;

/**
 * 
 */
public class RotateAngle extends Command {
	static double angle, initVal;
	//static final double TOLERANCE = 2;
	static TrajectoryController tc;
	private double isPos, curVal, curDisplacement = 0;
	//isPos: turns into a 1 is moving positive angle, -1 if moving negative angle
	//curVal: the current (NOT adjusted) value of our angle
	//curDisplacement: the adjustment factor due to gyro wraparound
	
	
	//IN DEGREES
    public RotateAngle(double theta) {
        // Use requires() here to declare subsystem dependencies
        // e.g. requires(chassis);
    	angle = theta;
    }

    // Called just before this Command runs the first time
    protected void initialize() {
    	initVal = RobotMap.imu.getYaw();
    	tc = new TrajectoryController(angle, 0.4, 0.4, 0.8, 0.9, -0.9); //TO-DO: would be nice to test these numbers!
    	isPos = Math.signum(angle);
    	curVal = initVal;
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
    	if (curVal * isPos > RobotMap.imu.getYaw() * isPos) //This will be true iff the gyro val has just wrapped around
    		curDisplacement += 180*isPos;
    			
    	RobotMap.driveTrain.arcadeDrive(0, tc.get(curVal + curDisplacement));
    	
    	curVal = RobotMap.imu.getYaw();
    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return (curVal + curDisplacement - initVal) * isPos > angle * isPos;
    }

    // Called once after isFinished returns true
    protected void end() {
    	RobotMap.driveTrain.arcadeDrive(0, 0);
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    }
{'fraction_non_alphanumeric': 0.09401231113598209, 'fraction_numerical': 0.024062674874090653, 'mean_word_length': 3.0395480225988702, 'pattern_counts': {'":': 0, '<': 4, '<?xml version=': 0, '>': 5, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '5833221', 'n_tokens_mistral': 1204, 'n_tokens_neox': 1103, 'n_words': 354}