Competition.c

#pragma config(UART_Usage, UART1, uartUserControl, baudRate115200, IOPins, None, None)
#pragma config(Sensor, in1,    intakePot,      sensorPotentiometer)
#pragma config(Sensor, in2,    liftPot,        sensorPotentiometer)
#pragma config(Sensor, dgtl1,  rightQuad,      sensorQuadEncoder)
#pragma config(Sensor, dgtl6,  leftQuad,       sensorQuadEncoder)
#pragma config(Sensor, dgtl8,  LED,            sensorDigitalOut)
#pragma config(Sensor, dgtl10, stopBtn,        sensorTouch)
#pragma config(Sensor, dgtl11, testBtn,        sensorTouch)
#pragma config(Sensor, dgtl12, liftStopButton, sensorTouch)
#pragma config(Motor,  port1,           liftRI,        tmotorVex393_HBridge, openLoop)
#pragma config(Motor,  port2,           intakeY,       tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port3,           driveLB,       tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port4,           driveLFY,      tmotorVex393_MC29, openLoop, encoderPort, dgtl6)
#pragma config(Motor,  port5,           driveRFY,      tmotorVex393_MC29, openLoop, encoderPort, dgtl1)
#pragma config(Motor,  port6,           liftLO,        tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port7,           driveRB,       tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port8,           liftRO,        tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port9,           liftLI,        tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port10,          lidar,         tmotorVex393_HBridge, openLoop, reversed, encoderPort, None)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma DebuggerWindows("debugStream")
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#define BCI_SEM_DEBUG_FULL_OFF
#define BCI_USE_UART
#define BCI_USE_TIMER
#define BCI_USE_POS_PID
#define BCI_USE_VEL_PID
#include "BCI\BCI.h"

#define UARTHANDLER_DEBUG_STD_MSG

//#define UARTHANDLER_DEBUG
//#define UARTHANDLER_DEBUG_READ
//#define MOVETOPOINT_DEBUG
//#define POINTMATH_DEBUG
//#define MPC_DEBUG
bool badData = false;
long autoStartTime = nPgmTime;
#include "uartHandler.c"
#include "pointMath.c"
#include "basicMotorControl.c"
#include "drivingFunctions.c"
#include "turningFunctions.c"
#include "intakeAndLiftHandler.c"
#include "collisionHandler.c"
#include "motorControl.c"
#include "decisionHandler.c"
#include "skills.c"
#include "noWarning.c"

#pragma platform(VEX2)
//#pragma competitionControl(Competition)

#include "WPICOMPETITION.c"

void pre_auton()
{
	SensorValue[LED] = 1;
  motor[lidar] = 55;
	bStopTasksBetweenModes = false;
	initSensors();
	initUART();
	intakeAndLiftTask_intakeState = INTAKE_REST;
	intakeAndLiftTask_liftState = LIFT_REST;
	startTask(intakeAndLiftTask);
	startTask(readBuffer);
	wait1Msec(250);
	motor[lidar] = 55;
}

task autonomous()
{
	autoStartTime = nPgmTime;
	motor[lidar] = 55;
	driveStraight_Ballsy(-550);
	intakeAndLiftTask_liftState = LIFT_REST;
	intakeAndLiftTask_intakeState = INTAKE_OPEN;
	pickUp(1500,750,true);


	intakeAndLiftTask_intakeState = INTAKE_CLOSED;
	intakeAndLiftTask_liftState = LIFT_DOWN;
	wait1Msec(650);
	intakeAndLiftTask_liftState = LIFT_HALF;
	wait1Msec(400);
	dumpIntake();

	intakeAndLiftTask_liftState = LIFT_DOWN;
	intakeAndLiftTask_intakeState = INTAKE_CUBE;
	pickUp(1450,350,true);


	intakeAndLiftTask_intakeState = INTAKE_CLOSED;
	intakeAndLiftTask_liftState = LIFT_DOWN;
	wait1Msec(650);
	driveStraight_Ballsy(-400);
	intakeAndLiftTask_liftState = LIFT_HALF;
	dumpIntake();

	startTask(commandRobot);
	wait1Msec(10000);
	stopTask(autonomous);
}


task usercontrol()
{
	autoStartTime = nPgmTime;
	motor[lidar] = 55;
	intakeAndLiftTask_intakeState = INTAKE_OPEN;
	intakeAndLiftTask_liftState = LIFT_DOWN;

	int stdX = 0, stdY = 0, stdTheta = 0;

	BCI_lockSem(std_msgSem, "usercontrol")
	{
		stdX = std_msg[STD_MSG_EST_X];
		stdY = std_msg[STD_MSG_EST_Y];
		stdTheta = std_msg[STD_MSG_EST_THETA];

		BCI_unlockSem(std_msgSem, "usercontrol")
	}

	int theta = stdTheta;
	int half = 1790;//half field in mm
	int quarter = 895;//quarter field
	int fakeX = stdX - half;
	int fakeY = stdY - quarter;// center point being center of our half the field
	int safeDistance = 460;

	bool fence = fakeY > quarter - safeDistance;
	bool back = -1*fakeY > quarter - safeDistance;
	bool right = fakeX > half - safeDistance;
	bool left = -1 * fakeX > half - safeDistance;


	startTask(commandRobot);
	while(true){
		autoStartTime = nPgmTime;
		wait1Msec(5000);
	}

}