Kevin's MSP 2013 Robot Arm

package com.sherolchen.msp;

import lejos.nxt.NXTRegulatedMotor;

/**
 * A simple interface to a KTong's ARM!!! MWAHAHAHA
 * But we can make it better...we have the technology
 */

public class NXTArmAndGripper {

    // Member variables used to keep track of various motors
    private NXTRegulatedMotor turningMotor;
    private boolean turnerReversed = false;
    private NXTRegulatedMotor liftingMotor;
    private boolean lifterReversed = false;
    private NXTRegulatedMotor grippingMotor;

    // Constructor which assumes an orientation to the various motors
    public NXTArmAndGripper(NXTRegulatedMotor turner,
                            NXTRegulatedMotor lifter,
                            NXTRegulatedMotor gripper) {
        turningMotor = turner;
        liftingMotor = turner;
        grippingMotor = gripper;
        // Assumes gripper starts off in closed position.
        grippingMotor.resetTachoCount();
    }

    // Constructor which does not assume an orientation to the various motors
    public NXTArmAndGripper(NXTRegulatedMotor turner, boolean turnReverse,
                            NXTRegulatedMotor lifter, boolean liftReverse,
                            NXTRegulatedMotor gripper, boolean gripReverse) {
        turningMotor = turner;
        turnerReversed = turnReverse;
        liftingMotor = lifter;
        lifterReversed = liftReverse;
        grippingMotor = gripper;
        // Assumes gripper starts off in closed position
        grippingMotor.resetTachoCount();
    }

    // Various methods used to control motors (function is obvious form name)
    // The methods are merely wrappers around functions provided by the motor
    // control functions in leJOS

    public void turnLeft() {
        if (turnerReversed) {
            turningMotor.backward();
        } else {
            turningMotor.forward();
        }
    }

    public void turnRight() {
        if (turnerReversed) {
            turningMotor.forward();
        } else {
            turningMotor.backward();
        }
    }

    public void stopTurning() {
        turningMotor.stop();
    }

    public void liftUp() {
        if (lifterReversed) {
            liftingMotor.forward();
        } else {
            liftingMotor.backward();
        }
    }

    public void liftDown() {
        if (lifterReversed) {
            liftingMotor.forward();
        } else {
            liftingMotor.backward();
        }
    }

    public void stopLifting() {
        liftingMotor.stop();
    }

    public void openGripper() {
        // TODO: Still need to calibrate
        grippingMotor.rotateTo(0, true);
    }

    public void closeGripper() {
        // TODO: Still need to calibrate
        grippingMotor.rotateTo(4, true);
    }
}