Kevin's MSP 2013 Robot Arm Control

package com.sherolchen.msp;

//A number of imports needed for manual control
import lejos.nxt.Button;
import lejos.nxt.ButtonListener;
import lejos.nxt.SensorPort;
import lejos.nxt.SensorPortListener;
import lejos.nxt.Motor;
import lejos.nxt.LCD;
import lejos.nxt.TouchSensor;
import com.sherolchen.msp.NXTArmAndGripper;

/**
* KTong's Robotic Arm
* -------------------
* This is from the laboratory of Kevin, the "mad scientist"
* LOG
* Day 1: The arm got built really fast. This LEGO company makes things really
*  easy. Fortunate for me since the storm is starting to roll in.
* Day 4: Got Eclipse and leJOS set up today. Took me a bit as snow is starting
*  to build up outside and my fingers are getting rather cold. The weather
*  forecast looks good for tomorrow though. Looking forward to a sunny day.
* Day 8: IT'S ALIVE!...sort of. It moves...sort of. I'll figure it out later.
*  The weather forecast took a turn for the worst so I got to start getting
*  ready for the snow storm of the year apparently.
* Day 10: Car broke down. No one in sight. Luckily I have some food rations
*  saved up from the last storm. They are past the expiration date but they
*  should be fine I think.
*
*  I haven't had much time to work on my arm but at least I added a touch
*  sensor.
* Day 17: Rations are dwindling. I can bearly tipe. It'sss getting rewally cold
*  iin the ccabin. And the arm issn't really werking write now.
* Day 3?: mwahahahaha HAHAHAAA HAHAHAHAHA ... ** cough **
*/
public class ArmManualControl {

    /**
     * A number of variables used for my robotic arm...mwahahahaha
     */
    // Determines which options to display first
    private static boolean inTurningMode = true;
    // The instance of the arm used to control the arm
    private static NXTArmAndGripper myArm = new NXTArmAndGripper(Motor.B, Motor.C, Motor.A);
    // Constant variables used for displaying the menu options on the NXT
    private final static int NUM_CHAR_LCD_X = 16;
    private final static int NUM_CHAR_LCD_Y = 8;
    private final static String TURN_MODE = "TURN";
    private final static String LEFT_OPTION = "LEFT";
    private final static String RIGHT_OPTION = "RIGHT";
    private final static String LIFT_MODE = "LEFT";
    private final static String UP_OPTION = "UP";
    private final static String DOWN_OPTION = "DOWN";

    /**
     * Function: main(String[] args)
     * Description:
     * The main code the sets up the listeners for the buttons on the NXT
     * brick and the touch sensor and commands the arm to react accordingly
     */
    public static void main(String[] args) throws Exception {

        // The initial display screen to wait for the user to push a button
        System.out.println("Kevin the Mad Scientist...");
        System.out.println("Press any key to start manual control of arm.");
        Button.waitForAnyPress();

        // Displays the first menu option
        LCD.setAutoRefresh(false);
        updateLCDDisplay();

        // Sets up the listener for the Enter button on the NXT brick
        Button.ENTER.addButtonListener( new ButtonListener() {
            // Called every time the enter button is pressed
            // Toggles between Turning and Lifting and updates the display
            public void buttonPressed( Button button) {
                if (inTurningMode) {
                    inTurningMode = false;
                } else {
                    inTurningMode = true;
                }
                updateLCDDisplay();
            }
            // Called every time the enter button is released
            // Shouldn't do anything
            public void buttonReleased( Button button) {
                updateLCDDisplay();
            }
        }
        );

        // Sets up the listener for the Left button on the NXT brick
        Button.LEFT.addButtonListener( new ButtonListener() {
            // Called every time the left button is pressed
            // Depending on the mode, either turns left or lifts up
            public void buttonPressed( Button button) {
                if (inTurningMode) {
                    myArm.liftDown();
                } else {
                    myArm.liftUp();
                }
            }
            // Called every time the enter button is released
            // Should stop the turning motor or lifting motor
            public void buttonReleased( Button button) {
                if (inTurningMode) {
                    myArm.stopTurning();
                } else {
                    myArm.stopLifting();
                }
            }
          }
        );

        // Sets up the listener for the Right button on the NXT brick
        Button.RIGHT.addButtonListener( new ButtonListener() {
            // Called every time the right button is pressed
            // Depending on the mode, either turns right or lifts down
            public void buttonPressed( Button button) {
                if (inTurningMode) {
                    myArm.turnLeft();
                } else {
                    myArm.liftUp();
                }
            }
            // Called every time the enter button is released
            // Should stop the turning motor or lifting motor
            public void buttonReleased( Button button) {
                if (inTurningMode) {
                    myArm.stopTurning();
                } else {
                    myArm.stopLifting();
                }
            }
          }
        );

        // Sets up the listener for the touch sensor
        // Assumes that the touch sensor is connected to port S4
        SensorPort.S4.addSensorPortListener(new SensorPortListener() {
            // This function gets called everytime touch sensor changes state
            public void stateChanged(SensorPort aSource, int aOldValue, int aNewValue) {
                if (aOldValue > aNewValue) {
                    myArm.openGripper();
                } else {
                    myArm.closeGripper();
                }
            }
        }
        );

        // Essentially waits until finished playing
        Button.ESCAPE.waitForPressAndRelease();

        // Closing screen. Say's goodbye, waits for a bit, and ends
        LCD.clear();
        System.out.println("Goodbye...");
        LCD.refresh();
        Thread.sleep(1000);
    }

    /**
     * Function: updateLCDDisplay()
     * Description:
     * Updates the LCD display to show the mode in which the controls are in.
     * If inTurningMode is true, the LCD displays the turning options.
     * Otherwise, the LCD displays the lifting options.
     */
    public static void updateLCDDisplay() {
        LCD.clear();    // Clears the LCD and gets it ready to be written to
        if (inTurningMode) { // In turning mode so should display turn options
            LCD.drawString(TURN_MODE, NUM_CHAR_LCD_X/2 - TURN_MODE.length()/2,
                                      NUM_CHAR_LCD_Y/2);
            LCD.drawString(LEFT_OPTION, 0, NUM_CHAR_LCD_Y-1);
            LCD.drawString(RIGHT_OPTION, NUM_CHAR_LCD_X-RIGHT_OPTION.length(),
                                         NUM_CHAR_LCD_Y-1);
        } else { // Not in turning mode so should display lift options
            LCD.drawString(LIFT_MODE, NUM_CHAR_LCD_X/2 - LIFT_MODE.length()/2,
                                      NUM_CHAR_LCD_Y/2);
            LCD.drawString(UP_OPTION, 0, NUM_CHAR_LCD_Y-1);
            LCD.drawString(DOWN_OPTION, NUM_CHAR_LCD_X-DOWN_OPTION.length(),
                                         NUM_CHAR_LCD_Y-1);
        }
        LCD.refresh();  // Actually updates the screen to show what was drawn
    }

}