View unanswered posts | View active topics It is currently Sat Oct 25, 2014 3:50 am






Reply to topic  [ 6 posts ] 
FTC Autonomous Aritificial Intelligence State Machines 
Author Message
Rookie

Joined: Fri Nov 29, 2013 12:12 pm
Posts: 14
Post 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?


Fri Nov 29, 2013 1:25 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post 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.


Fri Nov 29, 2013 4:46 pm
Profile
Rookie

Joined: Fri Nov 29, 2013 12:12 pm
Posts: 14
Post Re: FTC Autonomous Aritificial Intelligence State Machines
What do you mean by the last thing about the teleop sharing code?


Fri Nov 29, 2013 8:05 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post 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.


Fri Nov 29, 2013 8:48 pm
Profile
Rookie

Joined: Fri Nov 29, 2013 12:12 pm
Posts: 14
Post 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.


Sun Dec 01, 2013 10:10 pm
Profile
Guru
User avatar

Joined: Sun Nov 15, 2009 5:46 am
Posts: 1347
Post 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);
    }
}


Wed Dec 04, 2013 6:04 am
Profile
Display posts from previous:  Sort by  
Reply to topic   [ 6 posts ] 

Who is online

Users browsing this forum: No registered users and 2 guests


You cannot post new topics in this forum
You cannot reply to topics in this forum
You cannot edit your posts in this forum
You cannot delete your posts in this forum
You cannot post attachments in this forum

Search for:
Jump to:  



Powered by phpBB © 2000, 2002, 2005, 2007 phpBB Group.
Designed by ST Software for PTF.