[PROJECT] clawbot.c

#pragma config(Sensor, in1,    fright,         sensorLineFollower)
#pragma config(Sensor, in2,    fleft,          sensorLineFollower)
#pragma config(Sensor, in3,    lightsensor,    sensorReflection)
#pragma config(Sensor, dgtl1,  red,            sensorLEDtoVCC)
#pragma config(Sensor, dgtl2,  yellow,         sensorLEDtoVCC)
#pragma config(Sensor, dgtl3,  green,          sensorLEDtoVCC)
#pragma config(Sensor, dgtl4,  touchclaw,      sensorTouch)
#pragma config(Sensor, dgtl5,  sonar,          sensorSONAR_inch)
#pragma config(Motor,  port1,           mleft,         tmotorVex393_HBridge, openLoop)
#pragma config(Motor,  port3,           headlight,     tmotorVexFlashlight, openLoop, reversed)
#pragma config(Motor,  port6,           mclaw,         tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port7,           marm,          tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port10,          mright,        tmotorVex393_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

task main()
{
    turnLEDOff(green);
    turnLEDOff(yellow);
    turnLEDOff(red);
    while(1==1)
    {
        while(SensorValue(lightsensor)<501)
        {
            turnFlashlightOff(headlight);
            if(SensorValue(fleft)>1000)
            {
                turnLEDOff(green);
                turnLEDOn(yellow);
                startMotor(mleft,-32);
                startMotor(mright,-50);
            }
            if(SensorValue(fright)>1000)
            {
                turnLEDOff(green);
                turnLEDOn(red);
                startMotor(mleft,50);
                startMotor(mright,32);
            }
            else
            {
                turnLEDOn(green);
                turnLEDOff(yellow);
                startMotor(mleft,-32);
                startMotor(mright,32);
            }
        }
        if(SensorValue(touchclaw)>0)
        {
            turnLEDOff(green);
            turnLEDOn(yellow);
            turnLEDOff(red);
            turnFlashlightOn(headlight,-127);
            stopMotor(mleft);
            stopMotor(mright);
            startMotor(mclaw,-32);
            wait(1);
            stopMotor(mclaw);
            turnLEDOn(green);
            turnLEDOff(yellow);
            startMotor(mleft,90);
            startMotor(mright,-90);
            wait(5);
            stopMotor(mleft);
            stopMotor(mright);
            while(1==1)
            {
                turnFlashlightOff(headlight);
                if(SensorValue(sonar)<7.5) // inches
                {
                    turnLEDOn(green);
                    turnLEDOff(yellow);
                    stopMotor(mleft);
                    stopMotor(mright);
                    startMotor(marm,-100);
                    wait(2.25);
                    stopMotor(marm);
                    startMotor(mclaw,32);
                    wait(1);
                    turnLEDOn(yellow); // victory screech
                    turnLEDOn(red);
                    wait(0.5);
                    turnLEDOff(green);
                    turnLEDOff(yellow);
                    turnLEDOff(red);
                    wait(0.5);
                    turnLEDOn(green);
                    turnLEDOn(yellow);
                    turnLEDOn(red);
                    wait(0.5);
                    turnLEDOff(green);
                    turnLEDOff(yellow);
                    turnLEDOff(red);
                    wait(0.5);
                    turnLEDOn(green);
                    turnLEDOn(yellow);
                    turnLEDOn(red);
                    wait(0.5);
                    turnLEDOff(green);
                    turnLEDOff(yellow);
                    turnLEDOff(red);
                    wait(0.5);
                    turnLEDOn(green);
                    turnLEDOn(yellow);
                    turnLEDOn(red);
                    stopAllTasks();
                }
                else
                {
                    turnLEDOff(green);
                    turnLEDOn(yellow);
                    turnLEDOff(red);
                    startMotor(mleft,32);
                    startMotor(mright,-32);
                }
            }
        }
        else
        {
            turnLEDOn(green);
            turnLEDOff(yellow);
            turnLEDOff(red);
            turnFlashlightOn(headlight,-127);
            startMotor(mleft,-32);
            startMotor(mright,32);
        }
    }
}