// RobotBuilder Version: 1.0
//
// This file was generated by RobotBuilder. It contains sections of
// code that are automatically generated and assigned by robotbuilder.
// These sections will be updated in the future when you export to
// Java from RobotBuilder. Do not put any code or make any change in
// the blocks indicating autogenerated code or it will be lost on an
// update. Deleting the comments indicating the section will prevent
// it from being updated in the future.
package org.usfirst.frc330.Beachbot2014Java.commands;
import edu.wpi.first.wpilibj.command.Command;
import org.usfirst.frc330.Beachbot2014Java.Robot;
import org.usfirst.frc330.Beachbot2014Java.subsystems.Chassis;
/**
 *
 */
public class  DriveDistanceAtAngle extends DriveDistance{
    double angle;
    
    public DriveDistanceAtAngle()
    {
        this(0,0,0,0,true);
    }
    
    public DriveDistanceAtAngle(double distance, double angle)
    {
        this(distance, 0, angle, 0, true);
    }
    
    public DriveDistanceAtAngle(double distance, double tolerance, double angle, double timeout, boolean stopAtEnd)
    {
        super(distance, tolerance, timeout, stopAtEnd);
        this.angle = angle;
    }
    // Called just before this Command runs the first time
    protected void initialize() {
        super.initialize();
        if (!Robot.chassis.getShiftState())
        {
            Robot.chassis.gyroPID.setGainName(Chassis.DRIVELOW);
        }
        else
        {
            Robot.chassis.gyroPID.setGainName(Chassis.DRIVEHIGH);
        }
        Robot.chassis.gyroPID.setSetpoint(angle);
        Robot.chassis.gyroPID.enable();            
    }

    public Command copy() {
        return new DriveDistanceAtAngle();
    }
}

// RobotBuilder Version: 1.0
//
// This file was generated by RobotBuilder. It contains sections of
// code that are automatically generated and assigned by robotbuilder.
// These sections will be updated in the future when you export to
// Java from RobotBuilder. Do not put any code or make any change in
// the blocks indicating autogenerated code or it will be lost on an
// update. Deleting the comments indicating the section will prevent
// it from being updated in the future.
package org.usfirst.frc330.Beachbot2014Java.commands;
import edu.wpi.first.wpilibj.command.Command;
import org.usfirst.frc330.Beachbot2014Java.Robot;
import org.usfirst.frc330.Beachbot2014Java.subsystems.Chassis;
/**
 *
 */
public class  DriveDistanceAtAngle extends DriveDistance{
    double angle;
    
    public DriveDistanceAtAngle()
    {
        this(0,0,0,0,true);
    }
    
    public DriveDistanceAtAngle(double distance, double angle)
    {
        this(distance, 0, angle, 0, true);
    }
    
    public DriveDistanceAtAngle(double distance, double tolerance, double angle, double timeout, boolean stopAtEnd)
    {
        super(distance, tolerance, timeout, stopAtEnd);
        this.angle = angle;
    }
    // Called just before this Command runs the first time
    protected void initialize() {
        super.initialize();
        if (!Robot.chassis.getShiftState())
        {
            Robot.chassis.gyroPID.setGainName(Chassis.DRIVELOW);
        }
        else
        {
            Robot.chassis.gyroPID.setGainName(Chassis.DRIVEHIGH);
        }
        Robot.chassis.gyroPID.setSetpoint(angle);
        Robot.chassis.gyroPID.enable();            
    }
    
    public void setParam3(double angle) {
        this.angle = angle;
    }

    public Command copy() {
        return new DriveDistanceAtAngle();
    }
}

add angle as parameter 3