/*
 * DO NOT REMOVE THIS COPYRIGHT NOTICE
 * ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 *      By using this code, the user hereinafter agrees to
 *      abide by all the terms and conditions of the following
 *      agreement that nobody ever reads, as well as the
 *      Geneva Convention and the U.N. Charter and the Secret
 *      Membership Oath of the Benevolent Protective Order of
 *      the Elks and such other terms and conditions, real and
 *      imaginary, as the Software Company shall deem necessary
 *      and appropriate, including the right to come to the user's home
 *      and examine the user's hard drive, as well as the user's
 *      medicine cabinet if we feel like it, take it or leave it, until
 *      death do us part, one nation indivisible, by the dawn's early
 *      light,... finders keepers, losers weepers, thanks you've been a
 *      great crowd, and don't forget to tip your servers.
 *
 */

/*----------------------------------------------------------------------------*/

#pragma db
#include <reg52.h>
#include "lcd.h"

/*----------------------------------------------------------------------------*/

typedef unsigned char Byte;
typedef unsigned short Word;

/*----------------------------------------------------------------------------*/

sbit ARM_P5 = P1^2;
sbit ARM_P6 = P1^3;
sbit ARM_P7 = P1^4;
sbit ARM_P8 = P1^5;

/*----------------------------------------------------------------------------*/

void Sleep(void) {

        int i = 500;

        while( --i > 0 );
}

/*----------------------------------------------------------------------------*/

void MoveRobot(Byte axis,               /* [0-5] */
               Byte direction) {        /* [0,1] */

        static Byte ROBOT_QUICK_MOVE_TABLE[5][2] = {

                0x03, 0x02,     /* axis 1 */
                0x06, 0x09,     /* axis 2 */
                0x05, 0x04,     /* axis 3 */
                0x01, 0x00,     /* axis 4 */
                0x07, 0x08,     /* axis 5 */
        };

        /*
         * Note that, in the above table, I assume "up", "cw"
         * and "open" are positive moves and "down", "ccw", and
         * "close" are negative move.  Now, positive moves
         * correspond to direction set to "0" and negative
         * moves correspond to direction set to "1".  You should
         * be confused now!
         */

        /* argument checking */
        if( axis > 5 || direction > 1 ) return;

        ARM_P5 = ROBOT_QUICK_MOVE_TABLE[axis][direction] & 0x08 ? 1 : 0;
        ARM_P6 = ROBOT_QUICK_MOVE_TABLE[axis][direction] & 0x04 ? 1 : 0;
        ARM_P7 = ROBOT_QUICK_MOVE_TABLE[axis][direction] & 0x02 ? 1 : 0;
        ARM_P8 = ROBOT_QUICK_MOVE_TABLE[axis][direction] & 0x01 ? 1 : 0;
}

/*----------------------------------------------------------------------------*/

void StopRobot() {

        ARM_P5 = 1;
        ARM_P6 = 1;
        ARM_P7 = 1;
        ARM_P8 = 1;
}

/*----------------------------------------------------------------------------*/

bit GetButton1() {

        bit val;

        val = (P1 & 0x80) ? 1 : 0;
        while( P1 & 0x80 ); 
        return val;
}

/*----------------------------------------------------------------------------*/

bit GetButton2() {

        bit val;

        val = (P1 & 0x40) ? 1 : 0;
        while( P1 & 0x40 ); 
        return val;
}

/*----------------------------------------------------------------------------*/

void main( void ) {

	InitLCD();
	ClearScreen();

        MoveCursor(0, 0);
        PrintString("Ready...");
        while( GetButton1() == 0 );

	for(;;) {

                MoveCursor(0, 0);
                PrintString("Fall...");
                while( GetButton1() == 0 ) MoveRobot(2, 1), Sleep();
                while( GetButton1() == 0 ) MoveRobot(1, 1), Sleep();
                while( GetButton1() == 0 ) MoveRobot(2, 1), Sleep();
                StopRobot();

                MoveCursor(0, 0);
                PrintString("Grab...");
                while( GetButton1() == 0 ) MoveRobot(4, 1), Sleep();
                StopRobot();

                MoveCursor(0, 0);
                PrintString("Lift...");
                while( GetButton1() == 0 ) MoveRobot(2, 0), Sleep();
                while( GetButton1() == 0 ) MoveRobot(1, 0), Sleep();
                while( GetButton1() == 0 ) MoveRobot(2, 0), Sleep();
                StopRobot();

                MoveCursor(0, 0);
                PrintString("Swing...");
                while( GetButton1() == 0 ) MoveRobot(0, 1), Sleep();
                StopRobot();

                MoveCursor(0, 0);
                PrintString("Drop...");
                while( GetButton1() == 0 ) MoveRobot(4, 0), Sleep();
                StopRobot();

                MoveCursor(0, 0);
                PrintString("Home...");
                MoveCursor(1, 0); PutChar('0');
                while( GetButton2() == 0 ) MoveRobot(0, 0), Sleep();
                MoveCursor(1, 0); PutChar('1');
                while( GetButton2() == 0 ) MoveRobot(1, 0), Sleep();
                MoveCursor(1, 0); PutChar('2');
                while( GetButton2() == 0 ) MoveRobot(2, 0), Sleep();
                MoveCursor(1, 0); PutChar(' ');
                StopRobot();
	}
}