AutonomousFunctions

#pragma config(Hubs,  S1, HTMotor,  HTServo,  HTMotor,     none)
#pragma config(Sensor, S2,     ColorSensor,    sensorI2CHiTechnicColor)
#pragma config(Sensor, S2,     IRSeeker,       sensorHiTechnicIRSeeker1200)
#pragma config(Motor,  mtr_S1_C1_1,     Left,          tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     Right,         tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C3_1,     LinearSlide,   tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C3_2,     ArmMotor,      tmotorTetrix, openLoop, encoder)
#pragma config(Servo,  srvo_S1_C2_1,    ArmExtend,            tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C2_2,    ArmJoint,             tServoStandard)
#pragma config(Servo,  srvo_S1_C2_3,    servo3,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C2_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"
#include "hitechnic-colour-v2.h"

int wheelCircumference = 0;
int rampLength = 0;
float linSlideGearCircumference = 4*PI;
int linSlideHeight = 28;
int linSlideTicks = (1440 * linSlideHeight)/linSlideGearCircumference;
int armTicks;
int armJointTipped;
int armJointUp;

int red;
int blue;

bool armFullExtension = true;
bool bucketDrop = true;

#include "JoystickDriver.c"

void clear(){
	nMotorEncoder[Left] = 0;
	nMotorEncoder[Right] = 0;
	nMotorEncoder[LinearSlide] = 0;
	nMotorEncoder[ArmMotor] = 0;
}
task tipArm(){
	servo[ArmJoint] = armJointTipped;
	bucketDrop = true;
}
task resetArm(){
	servo[ArmJoint] = armJointUp;	
	bucketDrop = false;
}
void driveStraight(int in){
	while(nMotorEncoder[Left] < (in*(wheelCircumference/1440))){
		motor[Left]= 80;
		motor[Right] = 80;
	}
}
task raiseLinearSlide(){
	while(nMotorEncoder[LinearSlide] < linSlideTicks){
		motor[LinearSlide] = 80;
	}
}
task lowerLinearSlide(){
	while(nMotorEncoder[LinearSlide] > -linSlideTicks){
			motor[LinearSlide] = -80;
		}
}
task raiseArm(){
	while(nMotorEncoder[ArmMotor] < armTicks){
		motor[ArmMotor] = 70;
	}
}
task lowerArm(){
	while(nMotorEncoder[ArmMotor] > -armTicks){
		motor[ArmMotor] = -70;
	}
}
task extendArm(){
	while(HTCS2readColor(ColorSensor) != red){
		servo[ArmExtend] = 194;
	}
	servo[ArmExtend] = 127;
	armFullExtension = true;
}
task retractArm(){
	while(HTCS2readColor(ColorSensor) != blue){
		servo[ArmExtend] = 60;
	}
	servo[ArmExtend] = 127;
	armFullExtension = false;
}
void initiallize(){
	StartTask(retractArm);
	StartTask(resetArm);
}
task main(){
	initiallize();
	if(armFullExtension == false && bucketDrop == false){
		StopTask(retractArm);
		Stop Task(resetArm);
	}
 	waitForStart();
	
}