IRScoreF.c

#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTMotor,  HTServo)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     gyro,           sensorAnalogInactive)
#pragma config(Sensor, S3,     IR,             sensorI2CCustom)
#pragma config(Motor,  mtr_S1_C1_1,     rightDrive,    tmotorTetrix, PIDControl, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     leftDrive,     tmotorTetrix, PIDControl, reversed, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     lift,          tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_2,     motorG,        tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C3_1,     arm,           tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C3_2,     bucket,        tmotorTetrix, openLoop, encoder)
#pragma config(Servo,  srvo_S1_C4_1,    dumper,               tServoStandard)
#pragma config(Servo,  srvo_S1_C4_2,    flagMount,            tServoStandard)
#pragma config(Servo,  srvo_S1_C4_3,    flagRaiser,           tServoContinuousRotation)
#pragma config(Servo,  srvo_S1_C4_4,    servo4,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_5,    servo5,               tServoNone)
#pragma config(Servo,  srvo_S1_C4_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c"  //Include file to "handle" the Bluetooth messages.
#include "hitechnic-irseeker-v2.h" //includes irseeker drivers
#include "gyro.h"//include gyro drivers
#include "pidturn.h"//includes pidturns
#include "drive.h"//includes main library
#include "delay.h"//includes delay library

GYRO g_Gyro;
PIDTURN g_PidTurn;

const int MIN_TURN_POWER = 45;
const float TURN_KP = 0.85;//Default was 0.9
const float TURN_TOLERANCE = 0.3;

const int servoRestPosition = 233;

void initializeRobot(){
	servo[dumper] = servoRestPosition;
	servo[flagMount] = 17;
	return;
}

task main()
{
	int timeToWait = requestTimeToWait();
	initializeRobot();

	waitForStart(); // Wait for the beginning of autonomous phase.
	GyroInit(g_Gyro, gyro, 0);
	PidTurnInit(g_PidTurn, leftDrive, rightDrive, MIN_TURN_POWER, g_Gyro, TURN_KP, TURN_TOLERANCE);
	//Align against bottom wall, with left edge of left wheels on left edge of third tile (6ft from right wall).
	countdown(timeToWait);
	moveForwardInches(35, 1); //away from wall
	turn(g_PidTurn, 45, 20); //turn to parallel with buckets at 20% speed
	clearEncoders(); //clears encoder for the next step

	const int totalTics = 7078; //6840 total tics from before IR to end-- DONT CHANGE!
	const int ticsToCenter = 3979;
	const int ticsToSubtract = 1110;
	int ticsToIR;
	while(HTIRS2readACDir(IR) != 4 && (abs(nMotorEncoder[leftDrive])) < (totalTics - ticsToSubtract)){ //finds the beacon
		nxtDisplayCenteredTextLine(5,"Direction:%d",HTIRS2readACDir(IR));
		startForward(27);
		ticsToIR = nMotorEncoder[leftDrive];
		nxtDisplayCenteredTextLine(6,"%d",ticsToIR);
	}
	//stopDrive();
	//PlaySound(soundBeepBeep);
	stopDrive();
	wait1Msec(300);
	while(HTIRS2readACDir (IR) != 5 && (abs(nMotorEncoder[leftDrive])) < (totalTics-ticsToSubtract)){ //slow down to look for basket
		startForward(15);
	}
	//while(true){}
	stopDrive();//stops robot
	//wait1Msec(30000); //FOR DEBUGGING
	int currentPosition = abs(nMotorEncoder[leftDrive]);
	if(currentPosition < ticsToCenter)
		moveForwardInchesNoReset(30,6.75);
	else
			moveForwardInchesNoReset(30,3.75);
	//}

	servo[dumper] = 30;//dumps the block
	motor[lift]= 50;//starts the lift up
	wait1Msec(700);
	motor[lift]= 0;//stops lift
	servo[dumper] = servoRestPosition;//resets servo
	//while(true)
	wait1Msec(330);
	int ticsToMove= totalTics- nMotorEncoder[leftDrive];//tics left after IR
	moveForwardTics(75, ticsToMove); //move to end after IR
	turn(g_PidTurn, -68,60); //turn to go towards ramp

	moveForwardInches(75, 48); //forwards to ramp
	turn(g_PidTurn, -110, 60); //turn to face ramp
	moveForwardInches(90, 49);//onto ramp
	motor[lift]= -50;//starts the lift up
	wait1Msec(500);
	motor[lift]= 0;//stops lift
	while (true)
	{}
}