void FixedUpdate ()
{
var pos1 = new Vector3(-5, 0, 0);
var pos2 = new Vector3(5, 0, 0);
//Debug.Log(string.Format("Sequence at state {0}", state));
switch (state)
{
// Move to 0,0,0 position
case 0:
armControls.SetStaticPosition("idle");
if (movementControls.DriveTo(Vector3.zero, true) == 0)
{
state = 1;
}
break;
// Pick up ball (move towards it through pickup routine automatically if needed)
case 1:
if (pickupRoutine.Execute(ball) == 0)
{
state = 2;
}
break;
// Move towards first position, drop ball
case 2:
if (movementControls.DriveTo(pos1, true) == 0)
{
pickupRoutine.Drop();
state = 3;
}
break;
// Move to 0,0,0 position again
case 3:
armControls.SetStaticPosition("idle");
if (movementControls.DriveTo(Vector3.zero, true) == 0)
{
state = 4;
}
break;
// Pick up ball (move towards it through pickup routine automatically if needed)
case 4:
if (pickupRoutine.Execute(ball) == 0)
{
state = 5;
}
break;
// Move towards second position, drop ball, loop back to beginning if required
case 5:
if (movementControls.DriveTo(pos2, true) == 0)
{
pickupRoutine.Drop();
if (loop)
{
state = 0;
}
}
break;
}
}
public int Execute (GameObject target)
/*
Call this continuously to execute the routine
Return codes:
0: Routine complete
2: Routine in progress
*/
{
//Debug.Log(string.Format("Pickup at state {0}", state));
targetRb = target.GetComponent<Rigidbody> ();
targetTr = target.GetComponent<Transform> ();
switch(state)
{
// Move near the object to be picked up
case 0:
armControls.SetStaticPosition("idle");
if (movementControls.DriveTo(target.transform, true) == 0)
{
state = 1;
leftWheel.constraints = rightWheel.constraints = RigidbodyConstraints.FreezeRotation;
}
return 2;
case 1:
targetRb.velocity = Vector3.zero;
if (armControls.SetStaticPosition("sides") == 0)
{
state = 2;
}
return 2;
case 2:
if (armControls.SetStaticPosition("forwardL") == 0)
{
posDiff = leftHand.position - targetTr.position;
leftAttachJoint = leftHand.gameObject.AddComponent<FixedJoint> ();
rightAttachJoint = rightHand.gameObject.AddComponent<FixedJoint> ();
leftAttachJoint.connectedBody = targetRb;
rightAttachJoint.connectedBody = targetRb;
leftWheel.constraints = rightWheel.constraints = RigidbodyConstraints.None;
state = 3; // Move on to holding state, keep holding object in FixedUpdate
}
return 2;
}
return 0;
}
void FixedUpdate ()
{
switch (state)
{
// Move to starting position
case 0:
if (movementControls.DriveTo(destination, true) == 0)
{
// Destination calculated so that the ball is between the goal and the destination
destination = ball.transform.position + 2 * (ball.transform.position - goal).normalized;
state = 1;
}
break;
// Move to kicking position
case 1:
if (movementControls.DriveTo(destination, true) == 0)
{
obstacle.enabled = false;
state = 2;
// Bot moves towards the goal, hopefully hitting the ball in the right direction
destination = ball.transform.position - 1 * (ball.transform.position - goal).normalized;
}
break;
// Wait for ball to be available
// Agent seems to react to NavMesh changes with a delay, this seems to fix it
case 2:
if (!obstacle.enabled)
{
state = 3;
}
break;
// Bump the ball towards the goal
case 3:
// Calculates the difference in direction towards the ball and the goal
float offCourse = ((goal - pos.position).normalized - (ball.transform.position - pos.position).normalized).magnitude;
// Reposition behind the ball if too far off course
if (offCourse > 0.3f)
{
destination = ball.transform.position + 1 * (ball.transform.position - goal).normalized;
state = 1;
}
movementControls.DriveTo(ball.transform.position - 2 * (ball.transform.position - goal).normalized, true);
break;
case 4:
// Celebrate
armControls.SetStaticPosition("up");
break;
}
// Check if the ball has scored
if ((ball.transform.position - goal).magnitude < 1.8f)
{
state = 4;
}
}
public int Execute ()
/*
Call this continuously to execute the routine
Return codes:
0: Routine complete
2: Routine in progress
*/
{
// State-machine for the fall and rise routine
switch (fallAndRiseState)
{
// Routine start
case 0:
oldConstraints = robotBody.constraints;
robotBody.constraints = RigidbodyConstraints.None;
fallAndRiseStartTime = Time.time;
fallAndRiseState = 1;
break;
// No rotation restriction, falling down
case 1:
if (Mathf.Abs(Utils.AngleNorm180(robotBody.transform.eulerAngles.x)) > 45f)
{
fallAndRiseState = 2;
fallenTime = Time.time;
}
break;
// I've fallen and can't get up (angle above threshold), wait a few seconds to stabilize bot
case 2:
if ((Time.time - fallenTime) > 5f)
{
fallAndRiseState = 3;
}
break;
// First move arms to the sides (just in case they are in a weird position), then forward or backward depending on which side we fell
case 3:
if (armControls.SetStaticPosition("sides") == 0)
{
fallAndRiseState = 4;
}
break;
// Try to rise up using arms, lock wheels to prevent sliding
case 4:
foreach (Rigidbody rb in wheels)
{
rb.constraints = RigidbodyConstraints.FreezeRotation;
}
string pos = (Utils.AngleNorm180(robotBody.transform.eulerAngles.x) > 0) ? "forward" : "back";
armControls.SetStaticPosition(pos);
if (Mathf.Abs(Utils.AngleNorm180(robotBody.transform.eulerAngles.x)) < 30f)
{
fallAndRiseState = 5;
riseupStart = Time.time;
}
break;
// Move back upright
case 5:
robotBody.angularVelocity = Vector3.zero;
robotBody.velocity = Vector3.zero;
robotBody.constraints = oldConstraints;
robotBody.MoveRotation(Quaternion.Euler(Vector3.zero));
if (Time.time - riseupStart > 5f) // Assume we are up after 5s
{
foreach (Rigidbody rb in wheels)
{
rb.constraints = RigidbodyConstraints.None;
}
fallAndRiseState = 0; // Go back to beginning
return 0;
}
break;
}
return 2;
}
