;**Sumo Robot**

;**Variables**
temp 			var			word
PushButton		var			in10		
LeftBumper		var			in3			
RightBumper		var			in1
LineTracker		var			in9

;**Constants**
Scoop			con			p2
LeftMotor		con			p4
RightMotor		con			p0


;Initialise Motor Outputs
	High RightMotor
	High LeftMotor

Wait1:
	pause 200							;do nothing for 200ms

Wait2:	
	pulsout RightMotor,6000				;brake left motor	
	pulsout LeftMotor,6000				;brake right motor
	serout Scoop,i2400,[80]				;set sumo arm at position 80
	pause 8
	if PushButton = 0 then Wait2		;if button has not been pressed, jump to Wait2
	pause 150							;do nothing for 150ms
	
Start:				
	serout Scoop,i2400,[1]				;move scoop to pointing up
	pause 200							;do nothing for 200ms

Twist:
	serout RightMotor,i2400,["A",255]	;drive motor anticlockwise at 255 speed
	serout LeftMotor,i2400,["A",255]	;drive motor anticlockwise at 255 speed
	for temp = 1 to 100					;pause for 500ms but be able to see button input        
	pause 5								;result will be viper will spin for 500ms    
	if PushButton = 1 then Wait1		;but if the button is pressed it will go to Wait1
	next								;continue on with program
	pulsout RightMotor,6000				;brake motor
	pulsout LeftMotor,6000				;brake motor

Compete:
	serout RightMotor,i2400,["C",150]	;drive motor clockwise at 150 speed
	serout LeftMotor,i2400,["A",150]	;drive motor anticlockwise at 150 speed
		
Hunting:
	serout Scoop,i2400,[80]				;set servo at position 80	
	if LeftBumper = 1 then Charge		;if bump sensor detects bump then jump to Charge
	if RightBumper = 1 then Charge		;if bump sensor detects bump then jump to Charge
	pause 7								;do nothing for 7ms
	if LineTracker = 1 then Spin180		;if line tracker sensor detects edge then Spin180 
	pause 7								;do nothing for 7ms
	if PushButton = 1 then Wait1		;if the button is pressed then Wait1
	goto Hunting						;continue program from Hunting

Spin180:
	serout RightMotor,i2400,["A",150]	;drive the motor anticlockwise at 150 speed
	serout LeftMotor,i2400,["C",150]	;drive the motor clockwise at 150 speed
	for temp = 1 to 100					;Pause for 500ms but be able to see button input        
	pause 5								;    
	if PushButton = 1 then Wait1		;
	next								;continue on with program
	serout RightMotor,i2400,["C",200]	;drive the motor clockwise at 200 speed  
	serout LeftMotor,i2400,["C",200]	;drive the motor clockwise at 200 speed
	for temp = 1 to 100					;Pause for 500ms but be able to see button input        
	pause 5								;    
	if PushButton = 1 then Wait1		;
	next								;continue on with program					
	goto Compete						;go to Compete

Charge:
	serout RightMotor,i2400,["C",255]	;drive motor clockwise at 255 speed
	serout LeftMotor,i2400,["A",255]	;drive motor anticlockwise at 255 speed

LookingForEdge:
	serout Scoop,i2400,[1]				;set servo at position 1
	if LineTracker = 1 then Spin180		;if line tracker sensor detects edge then Spin180
	if PushButton = 1 then Wait1		;if the button is pressed then Wait1
	goto LookingForEdge					;continue program from LookingForEdge