ROBOTC.net forums
http://www.robotc.net/forums/

FTC Autonomous Aritificial Intelligence State Machines
http://www.robotc.net/forums/viewtopic.php?f=52&t=7386
Page 1 of 1

Author:  kkpanu9 [ Fri Nov 29, 2013 1:25 pm ]
Post subject:  FTC Autonomous Aritificial Intelligence State Machines

Just wondering. Would it be worthwhile to create a state machine program for my robot during the autonomous period of the FTC Competition? I don't see too many benefits of the state machines because it doesn't seem likely that my robot would go off of it's "happy path" and be disrupted. Do you guys have any ideas?

Author:  MHTS [ Fri Nov 29, 2013 4:46 pm ]
Post subject:  Re: FTC Autonomous Aritificial Intelligence State Machines

In general, state machines are very useful. Our team always uses state machines in our autonomous. It is extremely easy to code and allows the robot to multi-task. Since autonomous is short, it is super important to do multiple things at the same time (e.g. raise the arm while moving forward). The state machine also allows us to share common code with teleop.

Author:  kkpanu9 [ Fri Nov 29, 2013 8:05 pm ]
Post subject:  Re: FTC Autonomous Aritificial Intelligence State Machines

What do you mean by the last thing about the teleop sharing code?

Author:  MHTS [ Fri Nov 29, 2013 8:48 pm ]
Post subject:  Re: FTC Autonomous Aritificial Intelligence State Machines

A lot of teams wrote specific code for autonomous. It is so specific that it cannot be shared by teleop. If you partition the robot into subsystems (e.g. arm, scooper etc), these subsystems can be used in both autonomous and teleop. In order to share the subsystems between autonomous and teleop, one must write the subsystem code capable of multi-tasking (i.e. cannot block). In order to write code that doesn't block, you need to use a state machine.

Author:  kkpanu9 [ Sun Dec 01, 2013 10:10 pm ]
Post subject:  Re: FTC Autonomous Aritificial Intelligence State Machines

Also, what are the benefits of using a state machine instead of if and else if statements? Now that I think about it they seem very similar.

Author:  MHTS [ Wed Dec 04, 2013 6:04 am ]
Post subject:  Re: FTC Autonomous Aritificial Intelligence State Machines

By definition, state machines can remember their states. So when an operation takes a long time (e.g. drive forward 4 ft), instead of doing a "busy wait" loop just waiting for the drive to complete, a state machine just remembers the state and returns to the caller so that some other stuff can get done while the robot is driving forward. For example, without the state machine, the code may look like this:
Code:
void Drive(float distance)
{
    nMotorEncoder[leftMotor] = 0;
    nMotorEncoder[rightMotor] = 0;
    while (true)
    {
        float leftPos = nMotorEncoder[leftMotor]/ENCODER_COUNT_PER_INCH;
        float rightPos = nMotorEncoder[rightMotor]/ENCODER_COUNT_PER_INCH;
        if (abs(distance - (leftPos + rightPos)/2.0) <= DRIVE_TOLERANCE)
        {
            motor[leftMotor] = 0;
            motor[rightMotor] = 0;
            break;
        }
        motor[leftMotor] = DRIVE_KP*(distance - leftPos);
        motor[rightMotor] = DRIVE_KP*(distance - rightPos);
    }
}

void TurnShoulder(float angle)
{
    while (true)
    {
        float currAngle = nMotorEncoder[shoulderMotor]/ENCODER_COUNT_PER_DEGREE;
        if (abs(angle - currAngle) <= SHOULDER_TOLERANCE)
        {
            motor[shoulderMotor] = 0;
            break;
        }
        motor[shoulderMotor] = SHOULDER_KP*(angle - currAngle);
    }
}

void DumpBlock(int time)
{
    servo[scooper] = SCOOP_DOWN;
    wait1Msec(time);
    servo[scooper] = SCOOP_UP;
}

task main()
{
    TurnShoulder(60.0);     //raise shoulder to 60 degrees
    Drive(36.0);            //drive forward 36 inches
    DumpBlock(200);
}

Notice all the functions in this code have either busy wait loops or wait1Msec() statements. That means they don't return until the operations are completed. This is perfectly fine for a simple autonomous. However, imagine if you want to go park on the ramp after dumping the block and you are running out of time, you may want to do things in parallel to save some seconds. For example, there is no reason why you can't raise the shoulder and drive forward at the same time. How do you do that and still maintain the relatively simple looking main code? The answer is using a state machine.
Code:
long shoulderTarget = 0;
bool shoulderDone = true;

void SetShoulderTarget(float angle)
{
    shoulderTarget = angle*ENCODER_COUNT_PER_DEGREE;
    shoulderDone = false;
}

void ShoulderTask()
{
    if (!shoulderDone)
    {
        long currPos = nMotorEncoder[shoulderMotor];

        if (abs(shoulderTarget - currPos) <= SHOULDER_TOLERANCE)
        {
            // We are done.
            motor[shoulderMotor] = 0;
            shoulderDone = true;
        }
        else
        {
            motor[shoulderMotor] = SHOULDER_KP*(shouolderTarget - currPos);
        }
    }
}

long driveTarget = 0;
bool driveDone = true;

void SetDriveTarget(float distance)
{
    nMotorEncoder[leftMotor] = 0;
    nMotorEncoder[rightMotor] = 0;
    driveTarget = distance*ENCODER_COUNT_PER_INCH;
    driveDone = false;
}

void DriveTask()
{
    if (!driveDone)
    {
        long leftPos = nMotorEncoder[leftMotor];
        long rightPos = nMotorEncoder[rightMotor];

        if (abs(driveTarget - (leftPos + rightPos)/2) <= DRIVE_TOLERANCE)
        {
            // We are done.
            motor[leftMotor] = 0;
            motor[rightMotor] = 0;
            driveDone = true;
        }
        else
        {
            motor[leftMotor] = DRIVE_KP*(driveTarget - leftPos);
            motor[rightMotor] = DRIVE_KP*(driveTarget - rightPos);
        }
    }
}

task main()
{
    int state = 0;

    while (true)
    {
        switch (state)
        {
            case 0:
                // Start raising shoulder and driving forward.
                SetShoulderTarget(60.0);
                SetDriveTarget(36.0);
                state++;
                break;

            case 1:
                // Wait for driving done and shoulder in position.
                if (shoulderDone && driveDone)
                {
                    // Start dumping the block.
                    servo[scooper] = SCOOPER_DOWN;
                    time1[T1] = 0;
                    state++;
                }
                break;

            case 2:
                // Wait for 200 msec.
                if (time1[T1] >= 200)
                {
                    // turn scooper up and back up 1 foot.
                    servo[scooper] = SCOOPER_UP;
                    SetDriveTarget(-12.0);
                    state++;
                }
                break;

            case 3:
                if (driveDone)
                {
                    ....
                    ....
                }
                break;

                ...
                ...
        }
        DriveTask();
        ShoulderTask();
        wait1Msec(20);
    }
}

Page 1 of 1 All times are UTC - 5 hours [ DST ]
Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group
http://www.phpbb.com/