// WalkOne.java
// Test control program for WSU Robotic's 6 legged test robot.
// Copyright 2004 WSU Robotics
// Created 2004-10-17 by Dan Strother

// For reference, the robot's legs are arranged as such:
// (note that the front is the part with the acrylic sticking out farther)
//
//     +--+
// 3 --+  +-- 0
//     |  |
// 4 --+  +-- 1
//     |  |
// 5 --+--+-- 2
//
// And the motors are connected to the ASC16 like so (first servo is #1, not 0):
// ASC16.#1:  Leg 0 rotate
// ASC16.#2:  Leg 0 lift
// ASC16.#3:  Leg 1 rotate
// ASC16.#4:  Leg 1 lift
// ..etc..
// ASC16.#11: Leg 5 rotate
// ASC16.#12: Leg 5 lift


// Import namespace required for using Javelin's peripherals
import stamp.core.*;

public class WalkOne
{
  // Constants. These aren't used now.. ignore them.
  final static int    LEG_ROT_MOVE= 250;
  final static int    LEG_ROT_MID = 2000;
  final static int    LEG_MID     = 2000;
  final static int    LEG_MOVE    = 250;

  // Defines what servos control each leg's up/down motion.
  final static int    LEGSERVOS[] = {1,3,5,7,9,11};

  // Defines what servos control each leg's rotation
  final static int    LEGROTSERVOS[] = {0,2,4,6,8,10};

  // Defines which servos should have their directions inverted
  // (one side of the robot must be inverted with respect to the other)
  final static boolean SERVOINV[] = {false,false,false,false,false,false,
 //                                    false,false,false,false,false,false};
                                     true,true,true,true,true,true};

  // Temp variables.
  static int x,i;

  // UARTs for communicating with ASC16.
  static Uart tx = new Uart(Uart.dirTransmit, CPU.pin0, Uart.dontInvert, Uart.speed9600, 1);
  static Uart rx = new Uart(Uart.dirReceive, CPU.pin1, Uart.dontInvert, Uart.speed9600, 1);

  static void sendWord(int word)
  {
    // I don't have the slightest idea why this is required, but it was
    // necessary in order for the position coordinates to be centered
    // around 2000 (the range *should* be 0 to 4000).
    // We should really figure out why this is happening.
    word -= 1000;


	// Shift 'word' right by 8 bits to extract the MSB
    x = word >>> 8;

    // Send MSB
    tx.sendByte(x);

    // Send LSB (sendByte only sends the LSB anyways, so it doesn't matter that the MSB is still present)
    tx.sendByte(word);
  }

  // Raise specified leg up off the ground.
  static void legUp(int leg)
  {
    // Send move command.
    x = LEGSERVOS[leg]+1;
    tx.sendByte( x );

    // Invert coordinate as necessary.
    x = (SERVOINV[LEGSERVOS[leg]] ? 2000 : 2000);
    sendWord( x );
  }

  // Lower leg down to contact ground.
  static void legDown(int leg)
  {
    // Send move command.
    x = LEGSERVOS[leg]+1;
    tx.sendByte( x );

    // Invert coordinate as necessary.
    x = (SERVOINV[LEGSERVOS[leg]] ? 1000 : 3000);
    sendWord( x );
  }

  // Rotate leg to neutral position
  static void legRotMid(int leg)
  {
    // Send move command.
    x = LEGSERVOS[leg]+1;
    tx.sendByte( x );

    // Invert coordinate as necessary.
    x = (SERVOINV[LEGSERVOS[leg]] ? 2000 : 2000);
    sendWord( x );
  }

  // Rotate leg towards back of robot
  static void legRotBack(int leg)
  {
    // Send move command.
    x = LEGROTSERVOS[leg]+1;
    tx.sendByte( x );

    // Invert coordinate as necessary.
    x = (SERVOINV[LEGROTSERVOS[leg]] ? 1500 : 2500);
    sendWord( x );
  }

  // Rotate leg towards front of robot
  static void legRotFor(int leg)
  {
    // Send move command.
    x = LEGROTSERVOS[leg]+1;
    tx.sendByte( x );

    // Invert coordinate as necessary.
    x = (SERVOINV[LEGROTSERVOS[leg]] ? 2500 : 1500);
    sendWord( x );
  }

  // Waits for the specified leg's servos to stop moving (both lift and rotate)
  static void legWait(int leg)
  {
    x = 181+LEGSERVOS[leg];
    tx.sendByte( x );

    x = 181+LEGROTSERVOS[leg];
    tx.sendByte( x );
  }

  // Program entry point.
  public static void main()
  {
    System.out.println("Program begins now.");

    // Enable ASC16 (defaulst to ID of 255).
    tx.sendByte( 121 );
    tx.sendByte( 255 );

    // Initialize. Terminate and stop servo motion.
    tx.sendByte( 0 );
    tx.sendByte( 0 );
    tx.sendByte( 0 );
    tx.sendByte( 250 );

    // Enable servo control.
    tx.sendByte( 245 );

    // Set trigger level to 3 (pause and send message)
    tx.sendByte( 119 );
    tx.sendByte( 3 );

    // Invert servo coordinate systems as necessary.
    // This didn't seem to do what it should when I first tried it,
    // but reinvestigation may be required..
    /*for(i=0;i<12;++i)
    {
      if(SERVOINV[i])
        tx.sendByte( 112 );
      else
        tx.sendByte( 113 );

      x=i+1;
      tx.sendByte(x);
    }*/

    // Initialize servos
    for(i=0;i<12;i++)
    {
      // Set acceleration.
      x = i+81;
      tx.sendByte( x );
      tx.sendByte( 5 );

      // Set speed.
      x = i+61;
      tx.sendByte( x );
      tx.sendByte( 50 );

      // Set position to middle.
      x = i+1;
      tx.sendByte( x );
      sendWord( 2000 );
    }

    // Old test code
 /*   // Move legs up.
    for(i=0;i<6;++i)
      legUp(i);

    // Wait for motion to complete.
    tx.sendByte( 181 );

    // Move legs down.
    for(i=0;i<6;++i)
      legDown(i);*/

    /*legUp(0);
    legDown(1);
    legUp(2);
    legDown(3);
    legUp(4);
    legDown(5);

    legBack(0);
    legFor(1);
    legBack(2);
    legFor(3);
    legBack(4);
    legFor(5);*/

    // Move to flat resting position.
    for(i=0;i<6;++i)
    {
      legUp(i);
      legRotMid(i);
    }

    // Wait for legs to stop moving.
    legWait(0);
    legWait(1);

    // Move to upright resting position
    for(i=0;i<6;++i)
    {
      legDown(i);
    }

    // Wait for legs to stop moving.
    legWait(0);
    legWait(1);

    // Begin ASC16 loop. //////////////////////////////////
    tx.sendByte( 253 );

    // Lift even legs up.
    legUp(0);
    legUp(2);
    legUp(4);

    // Move odd legs back.
    legRotBack(1);
    legRotBack(3);
    legRotBack(5);

    // Move even legs forward.
    legRotFor(0);
    legRotFor(2);
    legRotFor(4);

    // Wait
    legWait(0);

    // Put even legs down.
    legDown(0);
    legDown(2);
    legDown(4);

    // Wait.
    legWait(0);

    // Lift odd legs up.
    legUp(1);
    legUp(3);
    legUp(5);

    // Move even legs back.
    legRotBack(0);
    legRotBack(2);
    legRotBack(4);

     // Move odd legs forward.
    legRotFor(1);
    legRotFor(3);
    legRotFor(5);

    // Wait
    legWait(1);

    // Put odd legs down.
    legDown(1);
    legDown(3);
    legDown(5);

    // Wait
    legWait(1);

    // End ASC16 loop.  //////////////////////////////////
    tx.sendByte( 254 );

    int result=0;

    // Echo ASC16 messages back to Javelin debug client.
    while(true)
    {
      result = rx.receiveByte();
      System.out.println(result);
    }

//    System.out.println("Program terminates now.");
  }
};