Full Code

CastleMainCode.c

#pragma config(Sensor, in1,    Castle1point1,  sensorLineFollower)
#pragma config(Sensor, in2,    Castle1point2,  sensorLineFollower)
#pragma config(Sensor, in3,    Castle1point3,  sensorLineFollower)
#pragma config(Sensor, in4,    Castle1king,    sensorReflection)
#pragma config(Sensor, dgtl1,  startbutton,    sensorTouch)
#pragma config(Sensor, dgtl2,  stopbutton,     sensorTouch)
#pragma config(Sensor, dgtl6,  castle1LED1,    sensorLEDtoVCC)
#pragma config(Sensor, dgtl7,  castle1LED2,    sensorLEDtoVCC)
#pragma config(Sensor, dgtl8,  castle1LED3,    sensorLEDtoVCC)
#pragma config(Motor,  port3,           servo1king,    tmotorServoStandard, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

int castle_score1;
int vstart;
int castle;
int stime;
int vtime;
int ftime;
float ptime;
int mtime;

task main()
{
	while(true)
	{
		clearLCDLine(0);
		clearLCDLine(1);

		if(SensorValue(startbutton) == 1)//If the start button is pressed
		{
			vstart = 1; //Change the variable start to 1
			clearTimer(T1);
			vtime = 0;
			ftime = 0;
			ptime = 0;
			mtime = 0;
			vtime = 0;
			castle_score1 = 0; //Set castle_score1 to 0
			setServo(servo1king, 110);
			while(vstart == 1) //While the variable start is equal to 1
			{
				displayLCDPos(0, 0);
				displayNextLCDString("Castle 1: ");
				displayLCDPos(0, 10);
				displayNextLCDNumber(castle_score1);
				displayLCDPos(0, 12);
				displayNextLCDString("pts");
				if (SensorValue(Castle1point1) < 2800) //If the limit switch1 is pressed (over the 1st wall)
				{
					castle_score1 += 3; 	//Add one to the score
					turnLEDOn(castle1LED1);
					wait(0.75);
					turnLEDOff(castle1LED1);
					wait(0.5);
					turnLEDOn(castle1LED1);
					wait(0.5);
					turnLEDOff(castle1LED1);
					wait(0.5);
				}

				if (SensorValue(Castle1point2) < 2800) //If the limit switch1 is pressed (over the 2nd wall)
				{
					castle_score1 += 1;
					turnLEDOn(castle1LED2);
					wait(0.5);
					turnLEDOff(castle1LED2);
					wait(0.5);
					turnLEDOn(castle1LED2);
					wait(0.5);
					turnLEDOff(castle1LED2);
				}

				if (SensorValue(Castle1point3) < 2800) //If the limit switch1 is pressed (over the 3rd wall)
				{
					castle_score1 += 5;
					turnLEDOn(castle1LED3);
					wait(0.5);
					turnLEDOff(castle1LED3);
					wait(0.5);
					turnLEDOn(castle1LED3);
					wait(0.5);
					turnLEDOff(castle1LED3);
				}
				if (SensorValue(Castle1king)<170)
				{
					setServo(servo1king, -15);
					stime = time1[T1];
					ptime = stime/1000;
					mtime = ptime/60;
					vtime = ptime - (60 * mtime);
					if (vtime == 0 || vtime == 1 || vtime == 2 || vtime == 3 || vtime == 4 || vtime == 5 || vtime == 6 || vtime == 7  || vtime == 8 || vtime == 9)
					{
						displayLCDPos(1, 0);
						displayNextLCDString("You Win!");
						displayLCDPos(1, 9);
						displayNextLCDNumber(mtime);
						displayLCDPos(1, 10);
						displayNextLCDString(":");
						displayLCDPos(1, 11);
						displayNextLCDString("0");
						displayLCDPos(1, 12);
						displayNextLCDNumber(vtime);
					}
					else
					{
						displayLCDPos(1, 0);
						displayNextLCDString("You Win!");
						displayLCDPos(1, 9);
						displayNextLCDNumber(mtime);
						displayLCDPos(1, 10);
						displayNextLCDString(":");
						displayLCDPos(1, 11);
						displayNextLCDNumber(vtime);
					}
					castle = 2;
					vstart = 0;
				}
				if (SensorValue(stopbutton)==1)
				{
					clearTimer(T1);
					vtime = 0;
					ftime = 0;
					ptime = 0;
					mtime = 0;
					vtime = 0;
					vstart = 0;
					castle_score1 = 0; //Set castle_score1 to 0
					setServo(servo1king, 110);
				}
			}
			if (castle == 2)
			{
				turnLEDOn(castle1LED1);
				turnLEDOn(castle1LED2);
				turnLEDOn(castle1LED3);
				wait(0.5);
				turnLEDOff(castle1LED1);
				turnLEDOff(castle1LED2);
				turnLEDOff(castle1LED3);
				wait(0.5);
				turnLEDOn(castle1LED1);
				turnLEDOn(castle1LED2);
				turnLEDOn(castle1LED3);
				wait(0.5);
				turnLEDOff(castle1LED1);
				turnLEDOff(castle1LED2);
				turnLEDOff(castle1LED3);
				wait(0.5);
				turnLEDOn(castle1LED1);
				turnLEDOn(castle1LED2);
				turnLEDOn(castle1LED3);
				wait(0.5);
				turnLEDOff(castle1LED1);
				turnLEDOff(castle1LED2);
				turnLEDOff(castle1LED3);
				wait(0.5);
				turnLEDOn(castle1LED1);
				turnLEDOn(castle1LED2);
				turnLEDOn(castle1LED3);
				wait(0.5);
				turnLEDOff(castle1LED1);
				turnLEDOff(castle1LED2);
				turnLEDOff(castle1LED3);
				wait(0.1);
				turnLEDOn(castle1LED1);
				wait(0.2);
				turnLEDOff(castle1LED1);
				wait(0.1);
				turnLEDOn(castle1LED2);
				wait(0.2);
				turnLEDOff(castle1LED2);
				wait(0.1);
				turnLEDOn(castle1LED3);
				wait(0.2);
				turnLEDOff(castle1LED3);
				wait(0.1);
				turnLEDOn(castle1LED2);
				wait(0.2);
				turnLEDOff(castle1LED2);
				wait(0.1);
				turnLEDOn(castle1LED1);
				wait(0.2);
				turnLEDOff(castle1LED1);
				wait(0.1);
				turnLEDOn(castle1LED2);
				wait(0.2);
				turnLEDOff(castle1LED2);
				wait(0.1);
				turnLEDOn(castle1LED3);
				wait(0.2);
				turnLEDOff(castle1LED3);
				wait(0.1);
				turnLEDOn(castle1LED2);
				wait(0.2);
				turnLEDOff(castle1LED2);
				wait(0.1);
				turnLEDOn(castle1LED1);
				wait(0.2);
				turnLEDOff(castle1LED1);
				wait(0.1);
				turnLEDOn(castle1LED2);
				wait(0.2);
				turnLEDOff(castle1LED2);
				wait(0.1);
				turnLEDOn(castle1LED3);
				wait(0.2);
				turnLEDOff(castle1LED3);
				wait(0.5);
				turnLEDOn(castle1LED1);
				turnLEDOn(castle1LED2);
				turnLEDOn(castle1LED3);
				wait(0.5);
				turnLEDOff(castle1LED1);
				turnLEDOff(castle1LED2);
				turnLEDOff(castle1LED3);
				wait(0.5);
				turnLEDOn(castle1LED1);
				turnLEDOn(castle1LED2);
				turnLEDOn(castle1LED3);
				wait(0.5);
				turnLEDOff(castle1LED1);
				turnLEDOff(castle1LED2);
				turnLEDOff(castle1LED3);
				if (SensorValue(stopbutton)==1)
				{
					clearTimer(T1);
					vtime = 0;
					ftime = 0;
					ptime = 0;
					mtime = 0;
					vtime = 0;
					vstart = 0;
					castle_score1 = 0; //Set castle_score1 to 0
					setServo(servo1king, 110);
				}
			}
			if (SensorValue(stopbutton)==1)
			{
					clearTimer(T1);
					vtime = 0;
					ftime = 0;
					ptime = 0;
					mtime = 0;
					vtime = 0;
					vstart = 0;
					castle_score1 = 0; //Set castle_score1 to 0
					setServo(servo1king, 110);
			}
		}
	}
}