import lejos.nxt.*;

/**
  seeing if the kluged shambler robot from Chapter 18
  of the course text can work...!

  @author Brian Bagnall p. 354 "Maximum Lego NXT"
  @author adapted by Sharon Tuttle
  @version 2015-03-26
*/

public class Crawler
{
    // data fields

    private final static int DOWN = 0;
    private final static int UP = -145;
    private final static int BACKWARD = 140;
    private final static int FORWARD = -BACKWARD;
    private final static double RATIO = 20.0/12.0;

    /**
     * test of walking gait
     * 
     * @param args not used
     */

    public static void main(String[] args)
    {
        // vertical lifter

        Motor.A.setSpeed(70);

        // lateral movement

        Motor.B.setSpeed(90);

        // rotation

        Motor.C.setSpeed(40);

        // is this 4 steps...?

        for (int i=0; i<4; i++)
	{
            stepForward(3);
            rotate(90);
        }
    }

    /**
       take a number of steps forward

       @param steps number of steps forward
    */

    public static void stepForward(int steps)
    {
        System.out.println("stepForward");
        for (int i=0; i<steps; i++)
        {
            System.out.print("1");
            Motor.B.rotate(FORWARD);
            System.out.print("2");
            Motor.A.rotateTo(UP);
            System.out.print("3");
            Motor.B.rotate(BACKWARD);
            System.out.print("4");
            Motor.A.rotateTo(DOWN);
            System.out.println("5");
        }
    }

    /**
       rotate the entire robot
 
       @param degrees how much to rotate the robot
    */

    public static void rotate(int degrees)
    {
        System.out.println("rotate");
        Motor.C.rotate((int) (RATIO * degrees));
        Motor.A.rotateTo(UP);
        Motor.C.rotate((int) (RATIO * -degrees));
        Motor.A.rotateTo(DOWN);
    }
}