Object: Ball //Generic Spherical Item
Object: Box //Generic Cubical item with one open side
General Definitions
Define Distance: The extent of space between two objects or places; an intervening space. Measured using the foot. //http://www.thefreedictionary.com/distance
Define Foot: A unit of length in the U.S. Customary and British Imperial systems equal to 12 inches (0.3048 meter) //http://www.thefreedictionary.com/foot
Define distance_to_box as Distance between Body and Ball
Define Global Variable 'StepsTaken' As Integer
Define Walk:
If distace_to_box < 1 foot Then exit loop
LLeg XferWeight
RLeg Lift(fwd)
StepsTaken += 1
RLeg XFerWeight
If distace_to_box < 1 foot Then exit loop
LLeg Lift(fwd)
StepsTaken += 1
End Define
Define RotateLeft(Degrees_to_turn as Integer):
Turn(L, Degrees_to_turn)
End Define
Define RotateRight(Degrees_to_turn as Integer):
Turn(R, Degrees_to_Turn)
End Define
Define SideStepL:
RLeg XferWeight
LLeg Lift Left
LLeg XFerWeight
RLeg Lower_To_Ground
End Define
-------
Main Program:
Begin Main()
Loop Until Body_Touch_Sensor = True
Loop Until STepsTaken =3
Walk
End Loop
StepsTaken = 0
:AfterSixStep
Loop Until StepsTaken >= 6
Walk
End Loop
RotateLeft(89)
StepsTaken = 0
Goto AfterSixStep
End loop
If Touching_Table = True and distance_to_box >= 1 foot Then
Loop until Perpendicular_to_Table = True
Turn(L, 1)
End loop
LowerRArm(Lift, fwd)
Loop Until Body_touch_Sensor = False OR Hand_Touch_Sensor = True
If Body_Touch_Sensor = False Then
Loop Until Hand_Touch_Sensor = True
SideStepR
End loop
UpperRArm(Lift Fwd to 45deg toward Floor_Plane)
UpperRArm(Rotate 45deg Anti-Clockwise)
Loop Until RHand is Inside Box
LowerRArm(Lower 5 deg)
End Loop
Call FindBall
End
End If
If Hand_Touch_Sensor = True Then
UpperRArm(Lift Fwd to 45deg toward Floor_Plane)
UpperRArm(Rotate 45deg Clockwise)
Loop Until RHand is Inside Box
LowerRArm(Lower 5 deg)
End Loop
Call FindBall
End
End If
End Loop
End if
If distance_to_box < 1 foot Then
Call Take_Ball
End If
Loop until distace_to_box < 1 foot
Walk
End Loop
End
End Main