Robot C Vex 3

gistfile1.c

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(122)

#include "Vex_Competition_Includes.c"

typedef struct {
  int left;
  int right;
  int arm;
  int lift;
  int servo;
  bool forkTop;
  bool forkBottom;
} Robot_Struct;

Robot_Struct Robot;

void pre_auton(){}

void useTank(){
  Robot.left=vexRT[Ch2];
  Robot.right=vexRT[Ch3];
}

void moveArm(){
  Robot.arm=vexRT[Ch3Xmtr2];
}

void fixForkLift(){
  if (Robot.forkTop && Robot.lift<0)
    Robot.lift=0;
  if (Robot.forkBottom && Robot.lift>0)
    Robot.lift=0;
}

void moveForkLift(){
  Robot.lift=vexRT[Ch2Xmtr2];
  fixForkLift();
}

void useServo() {
  Robot.servo=-32;
  if (vexRT[Btn6UXmtr2])
    Robot.servo=96;
  else if (vexRT[Btn6DXmtr2])
    Robot.servo=-64;
}

void getRobot(){
  Robot.forkTop=!SensorValue(dgtl1);
  Robot.forkBottom=!SensorValue(dgtl2);
}

void setRobot(){
  motor[port1]=Robot.left;
  motor[port2]=Robot.right;
  motor[port4]=-Robot.arm;
  motor[port5]=motor[port4];
  motor[port3]=-Robot.lift;
  motor[port6]=Robot.servo;
}

task usercontrol()
{
  Robot.left=0;
  Robot.right=0;
  Robot.arm=0;
  Robot.lift=0;
  Robot.servo=0;
  Robot.forkTop=false;
  Robot.forkBottom=false;

	while (true)
	{
	  getRobot();
    useTank();
    moveArm();
    moveForkLift();
    useServo();
    setRobot();
	}
}

task autonomous()
{
  Robot.left=0;
  Robot.right=0;
  Robot.arm=0;
  Robot.lift=0;
  Robot.servo=0;
  Robot.forkTop=false;
  Robot.forkBottom=false;
	while (true)
	{
	  getRobot();
	  if (!Robot.forkBottom) {
	    Robot.lift=127;
	  }
    fixForkLift();
    Robot.arm=Robot.lift;
    setRobot();
	}
}