/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package org.usfirst.frc330.Beachbot2013Java.commands;

import edu.wpi.first.wpilibj.command.Command;
import org.usfirst.frc330.Beachbot2013Java.Robot;

/**
 *
 * @author joe
 */
public class DriveWaypointBackward extends DriveWaypoint {

    public DriveWaypointBackward(double x, double y, double tolerance, double timeout, boolean stopAtEnd) {
        super(x, y, tolerance, timeout, stopAtEnd);
    }

    protected void calcXY(double x, double y) {
        double gyroAngle;
        
        super.calcXY(x, y);

        leftDistance = -leftDistance;
        rightDistance = -rightDistance;
        
        gyroAngle = Robot.chassis.getAngle();
        if (gyroAngle < angle)
            angle = angle-180;
        else
            angle = angle+180;
        System.out.println("Backward Angle: " + angle);
    }

    public Command copy() {
        return new DriveWaypointBackward(0,0,0,0,true);
    }
    
}

@@ -255,21 +255,1315 @@
 *%0A *
-%0A * @author j
+ Drive the robot backward to a specified waypoint (relative to where the robot started) %0A * using the encoders and the gyro to keep straight and ramp up to full speed. Finish when%0A * one of the encoders is within the specified %7B@link #setParam3(double) tolerance%7D. %0A * The %7B@link #setParam2(double) Y%7D location is positive to the front of the robot. %0A * The %7B@link #setParam1(double) X%7D location is positive to the right of the robot.%0A * The robot's current angle should be close to the angle to drive to the waypoint%0A * (ie this command is not appropriate for both turning and driving).%0A * If the robot needs to turn prior to driving to the waypoint, use %7B@link TurnWaypoint%7D %0A * first.%0A * %3Cp%3EThis is similar to DriveWaypoint, except that the robot drives backwards%0A * (pickup first).%0A * %3Cp%3E%0A * For example, to drive 10 feet forward from where the robot started, set Y to %0A * 120 (inches) and set X to 0.%0A * A reasonable tolerance is 3 inches for normal movements. This will stop the robot%0A * when it is between 117 - 123 inches. If a smaller%0A * tolerance is used, the robot may not ever reach the tolerance, and the %0A * %7B@link #setTimeout(double) timeout%7D may be exceeded. This will slow down the%0A * execution of future commands.%0A * %0A * @see DriveWaypoint%0A * @see TurnGyroWaypointBackward%0A * %0A * @author J
 oe%0A 

add javadoc