Problem_03.c

#pragma config(Sensor, in1,    PotLift,        sensorPotentiometer)
#pragma config(Sensor, in2,    teamSelection,  sensorPotentiometer)
#pragma config(Sensor, dgtl1,  EncoderDriveLeft, sensorQuadEncoder)
#pragma config(Sensor, dgtl3,  EncoderDriveRight, sensorQuadEncoder)
#pragma config(Motor,  port2,           DriveLeft,     tmotorVex393, openLoop)
#pragma config(Motor,  port5,           Lift,          tmotorVex393, openLoop)
#pragma config(Motor,  port9,           DriveRight,    tmotorVex393, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

/*
Michael Wallace
6/15/2015
MT SAC VEX ROBOTICS Guide to new robotic programmers

Licensed to the Apache Software Foundation (ASF) under one
or more contributor license agreements.  See the NOTICE file
distributed with this work for additional information
regarding copyright ownership.  The ASF licenses this file
to you under the Apache License, Version 2.0 (the
"License"); you may not use this file except in compliance
with the License.  You may obtain a copy of the License at

  http://www.apache.org/licenses/LICENSE-2.0

Unless required by applicable law or agreed to in writing,
software distributed under the License is distributed on an
"AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
KIND, either express or implied.  See the License for the
specific language governing permissions and limitations
under the License.
*/

int const DEFAULAT_TIME_OUT_MS = 5000;//5 seconds

#include "Robot_Functions.h"

//timer 2 is for lift control

int teamSelectionFunction();//example how to use a pot for team or routine selection

//function to change lift position by pot position and check for time out
bool liftFunction(int power, int position, int const DEFAULT_TIME_OUT = DEFAULAT_TIME_OUT_MS);
void liftFunction(int power);//primitive function to just change power of lift motors


task main()
{
	int const MAX_LIFT_HEIGHT = 1000;
	int const MIN_LIFT_HEIGHT = 100;

	liftFunction(100, MAX_LIFT_HEIGHT);//raise lift
	wait1Msec(5000);

	liftFunction(100, MIN_LIFT_HEIGHT);//lower lift
	wait1Msec(5000);

	liftFunction(0);//shut off lift


}

int teamSelectionFunction()//function to show how to use pot for team selection
{
 	if(SensorValue(teamSelection) >= 2014)//blue
	{
		//blueVersion1();
		//or you can return a value 1
	}
	else//red
	{
		//redVersion1();
	//or you can return a value 2 and so on
	}

	return 0;
}


//function to change lift position by pot position and check for time out
bool liftFunction(int power, int position, int const DEFAULT_TIME_OUT)
{
	ClearTimer(T2);//clear T2 timer

	if(SensorValue(PotLift) < position)//check if lift needs to go up
	{
		while(SensorValue(PotLift) < position)//loop until position it met or time out
		{
			if(time1[T2] > DEFAULT_TIME_OUT)//check if robot has take too long to reach distance
			{
				liftFunction(20);//hold lift maybe shut it off what ever you want
				return false;//destiantion failed timed out return false
			}
			liftFunction(abs(power));//power motor to lifts up
		}
	}
	else if (SensorValue(PotLift) > position)//check if lift needs to go up
	{
		while(SensorValue(PotLift) > position)//loop until position it met or time out
		{
			if(time1[T2] > DEFAULT_TIME_OUT)//check if robot has take too long to reach distance
			{
				liftFunction(20);//hold lift maybe shut it off what ever you want
				return false;//destiantion failed timed out return false
			}

			liftFunction(-abs(power));//power motors to lift down
		}
	}
	//else must be in the correct position or some thing is wrong

	//power to hold lift up
	liftFunction(20);
	return true;


}

//primitive function to just change power of lift motors
void liftFunction(int power)
{
	motor[Lift] = power;
}