import state.*;

class wheelTestThread extends Thread {
  RSPFrame rframe;

  wheelTestThread(RSPFrame r) {
    rframe = r;
  }

  public void run() {
    for(int x=0; x<=50; x++) {  // Speed from 0 -> 50 cm/s
      rframe.setRobotSpeed(x);
      try { Thread.sleep(10); } catch(InterruptedException ie) {}
    }
    for(int x=50; x>=-50; x--) {  // Speed from 50 -> -50 cm/s
      rframe.setRobotSpeed(x);
      try { Thread.sleep(10); } catch(InterruptedException ie) {}
    }
    for(int x=-50; x<=0; x++) {  // Speed from -50 -> 0 cm/s
      rframe.setRobotSpeed(x);
      try { Thread.sleep(10); } catch(InterruptedException ie) {}
    }
    rframe.setRobotSpeed(0);

    for(int x=0; x<=50; x++) {  // Wheel Speed from 0 -> 50 cm/s
      rframe.setWheelSpeed(0, x, 0, x);
      try { Thread.sleep(10); } catch(InterruptedException ie) {}
    }
    for(int x=50; x>=-50; x--) {  // Wheel Speed from 50 -> -50 cm/s
      rframe.setWheelSpeed(0, x, 0, x);
      try { Thread.sleep(10); } catch(InterruptedException ie) {}
    }
    for(int x=-50; x<=0; x++) {  // Wheel Speed from -50 -> 0 cm/s
      rframe.setWheelSpeed(0, x, 0, x);
      try { Thread.sleep(10); } catch(InterruptedException ie) {}
    }
    rframe.setWheelSpeed(0, 0, 0, 0);

    for(int x=0; x<=50; x++) {  // Wheel Speed from 0 -> 50 cm/s
      rframe.setWheelSpeed(x, 0, x, 0);
      try { Thread.sleep(10); } catch(InterruptedException ie) {}
    }
    for(int x=50; x>=-50; x--) {  // Wheel Speed from 50 -> -50 cm/s
      rframe.setWheelSpeed(x, 0, x, 0);
      try { Thread.sleep(10); } catch(InterruptedException ie) {}
    }
    for(int x=-50; x<=0; x++) {  // Wheel Speed from -50 -> 0 cm/s
      rframe.setWheelSpeed(x, 0, x, 0);
      try { Thread.sleep(10); } catch(InterruptedException ie) {}
    }
    rframe.setWheelSpeed(0, 0, 0, 0);

    // fault amps, one at a time
    rframe.setAmps(true, true, true, true, true, true,
		   false, false, false, false, false, true);
    try { Thread.sleep(500); } catch(InterruptedException ie) {}

    rframe.setAmps(true, true, true, true, true, true,
		   false, false, false, false, true, false);
    try { Thread.sleep(500); } catch(InterruptedException ie) {}

    rframe.setAmps(true, true, true, true, true, true,
		   false, false, false, true, false, false);
    try { Thread.sleep(500); } catch(InterruptedException ie) {}

    rframe.setAmps(true, true, true, true, true, true,
		   false, false, true, false, false, false);
    try { Thread.sleep(500); } catch(InterruptedException ie) {}

    rframe.setAmps(true, true, true, true, true, true,
		   false, true, false, false, false, false);
    try { Thread.sleep(500); } catch(InterruptedException ie) {}

    rframe.setAmps(true, true, true, true, true, true,
		   true, false, false, false, false, false);
    try { Thread.sleep(500); } catch(InterruptedException ie) {}

    // reset amps to enabled
    rframe.setAmps(true, true, true, true, true, true,
		   false, false, false, false, false, false);
    try { Thread.sleep(500); } catch(InterruptedException ie) {}

    // disable amps, one at a time
    rframe.setAmps(true, true, true, true, true, false,
		   false, false, false, false, false, false);
    try { Thread.sleep(500); } catch(InterruptedException ie) {}

    rframe.setAmps(true, true, true, true, false, true,
		   false, false, false, false, false, false);
    try { Thread.sleep(500); } catch(InterruptedException ie) {}

    rframe.setAmps(true, true, true, false, true, true,
		   false, false, false, false, false, false);
    try { Thread.sleep(500); } catch(InterruptedException ie) {}

    rframe.setAmps(true, true, false, true, true, true,
		   false, false, false, false, false, false);
    try { Thread.sleep(500); } catch(InterruptedException ie) {}

    rframe.setAmps(true, false, true, true, true, true,
		   false, false, false, false, false, false);
    try { Thread.sleep(500); } catch(InterruptedException ie) {}

    rframe.setAmps(false, true, true, true, true, true,
		   false, false, false, false, false, false);
    try { Thread.sleep(500); } catch(InterruptedException ie) {}

    // reset amps to enabled
    rframe.setAmps(true, true, true, true, true, true,
		   false, false, false, false, false, false);
    try { Thread.sleep(500); } catch(InterruptedException ie) {}
  }
}