#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Sensor, S2,     touchMPX,            sensorHiTechnicTouchMux)
#pragma config(Sensor, S3,     compass,             sensorI2CHiTechnicCompass)
#pragma config(Sensor, S4,     auto3,               sensorTouch)
#pragma config(Motor,  motorA,          clawMotor,     tmotorNormal, PIDControl)
#pragma config(Motor,  mtr_S1_C1_1,     leftMotor,     tmotorNormal, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     rightMotor,    tmotorNormal, openLoop, reversed)
#pragma config(Motor,  mtr_S1_C2_1,     armMotor,      tmotorNormal, PIDControl)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorNormal, PIDControl)
#pragma config(Servo,  srvo_S1_C3_1,    armServo,             tServoNormal)
#pragma config(Servo,  srvo_S1_C3_2,    armServo2,             tServoNormal)

#include "JoystickDriver.c"

void autofun1()
{
    nxtDisplayTextLine(2, "Auto 1");
    nMotorEncoder[rightMotor] = 0;
    nxtDisplayTextLine(2, "ME = %d", nMotorEncoder[rightMotor]);
    wait1Msec(1000);
    do
    {
        motor[leftMotor] = 100;
        motor[rightMotor] = 100;
        nxtDisplayTextLine(2, "ME = %d", nMotorEncoder[rightMotor]);
    }while(nMotorEncoder[rightMotor] < 2500);
    motor[leftMotor] = 0;
    motor[rightMotor] = 0;
    wait1Msec(800);
    motor[leftMotor] = -80;
    motor[rightMotor] = 100;
    wait1Msec(730);
    nMotorEncoder[rightMotor] = 0;
    do
    {
        motor[leftMotor] = 100;
        motor[rightMotor] = 100;
    }while(nMotorEncoder[rightMotor] < 4000);
    motor[leftMotor] = 0;
    motor[rightMotor] = 0;
    wait1Msec(800);
    nMotorEncoder[armMotor] = 0;   
    nxtDisplayTextLine(2, "Cut off 1"); //I see this
    wait1Msec(200);
    motor[armMotor] = -100; //This does not appear to execute
    nxtDisplayTextLine(2, "Cut off 2"); //And this
    wait1Msec(300);
    nxtDisplayTextLine(2, "Cut off 3"); //I very briefly see this
    motor[armMotor]= 0;
    nxtDisplayTextLine(2, "Cut off 4"); //And this
    wait10Msec(10);
}

void initializeRobot()
{
    motor[leftMotor] = 0;
    motor[rightMotor] = 0;
    return;
}

task main()
{
    initializeRobot();

    waitForStart();
    autofun1();
}