norviewfirstrobotics / frcstronghold2016 Goto Github PK

When setting up the ultrasonics, having the "automatic mode" set to false would stop the echo signal from sending anything. I will be debugging this more tomorrow in my free block, but I am very confused by it

This might require extracting a common interface and creating a separate class for the FrontOnly variant. We will want to use this:

• When in teleop mode to tell use how far we are from objects.
• Part of an extended autonomy routine (see separate issue)

(note: we will want to update the dashboard to provide red/magenta indications of whether we are proper distance from Portcullis, Cheval, and Boulder. If possible, provide a relative distance required to be in position, such as "-4.5" to mean move back 4 1/2 inches to be the right distance away.)

(note: originally, I was using the word "scenarios", but I think "activities" is probably more accurate.)

Thinking through the design, here are the activities that see the bot doing:

• Detect defense type (necessary for autonomy mode only)
• Breach low bar
• Breach rough terrain
• Breach high wall
• Breach moat
• Breach ramparts
• Breach portcullis (stretch goal)
• Breach cheval (stretch goal)
• Turn clockwise
• Turn counterclockwise
• Collect boulder (with arm)
• Deposit boulder

In teleop mode, I guess we also need a general purpose move/drive activity. We could probably use this as the default activity, since nothing would happen until the driver moved the joystick. If we don't have this as the default activity, then we should probably have an "idle" activity.

Note that these activities are high-level concepts. We are not worrying about small things (like running motors, positioning arm at a given position, etc). Autonomy mode would string together a number of activities to do something. In teleop mode, we would have a button for each of the activities, where the driver is the one who's choosing what to do next.

So, what other activities would we need?

When a match starts, do they start in the Disabled mode? Or does it always progress:

Start->Auto -> Teleop -> Disabled

(in this case Start is just the initial condition while it waits for the first "Auto" message from the control station.)

Also, I vaguely recall that the robot could be set to disabled at any time. So I assume these are also possible transitions:

Start -> Auto -> Disabled
Start -> Disabled

If that's the case, then I think that autonomousInit() can simply be:

setActivity(getDefaultActivity)

Leave teleopInit() empty. We'll just let the autonomy code implicitly switch to ManualDrive when it calls getDefaultActivity(). That way, as I mentioned in the other thread, autonomy can finish whatever its current activity is.

In disabledInit(), it can just be:

currentActivity.Cancel()

This will both stop whatever the current activity is and set the current activity to Idle (assuming that Cancel() calls robot.setActivity(robot.getDefaultActivity())).

Allow ability to set relative number of degrees of rotation (positive value is clockwise).

The objective is to move the bucket at full speed, but for less time. We will aim to throw it in an arc that can put the boulder in the low goal.

Always use the "abstract" keyword on a class that you do not intend to instantiate directly.

Let's take the BreachSimpleDefense activity and see how we might accomplish it. Here is one possible approach:

1. Move until FWD sensors detect both shields
2. Move until AFT sensors detect both shields
3. Move until FWD sensors detect both shields
4. Move until AFT sensors detect both shields
5. Move for 2 seconds
6. Stop

Suppose we were to define each of the steps above as their own activity:

1. ApproachDefense
2. EnterDefense
3. ExitDefense
4. ClearDefense
5. MoveForward

Now, we don't really want to change Robot.currentActivity because we would then have to keep track of nested activity levels in the robot (i.e. push the current activity on a stack, pop it back off again later). So... how can we execute these simple activities and still keep track of the fact that our top-level activity is BreachSimpleDefense?

One way is to make BreachSimpleDefense just like Robot, but implementing the IRobotActivity interface! Now, each time that BreachSimpleDefense.update is called (via Robot.currentActivity.update()), we turn around and call update() on its own currentActivity member! Note, however, that this also requires a "default" activity that encapsulates the steps above (just like the autonomy activity provides for the robot).

Now, having to implement the "default" activity (both at this level and at the Robot level) might be cumbersome. So, suppose we were to change the IRobotActivity interface slightly by adding a method:

bool isComplete()

With this, we can still allow some activities (such as DetectDefenseType) to call SetActivity when it needs to specifically change the activity. But for the other activities, we would simply stop processing and have IsComplete() return true. Now, the iterative method (in Robot) can look something like this:

if(! currentActivity.isComplete())
{
    currentActivity.Update();
}
else
{
    // code to determine the next appropriate activity...
}

If you want, you can encapsulate this in your GameState classes for the top level (instead of having an Autonomy, ManualDrive, and Idle activity).

In an activity like BreachSimpleDefense, you will have the same code inside update(), except that the else branch would obviously contain code to go through steps 1 to 5 above (step 6 would be setting isComplete to true).

Either way, the idea is to reuse activity concept for sub-activities. Each activity just uses the sub-activities in different combinations.

Instead of "Move" at the end of the autonomous breach, use a new Advance subactivity. This will use the ForwardOnly ultrasonic to approach the back wall until we are within some distance, then stop.

Note: This assumes the availability of Turn, Advance, and ShootBoulder subroutines.

The idea is as follows:

After a breach, advance to the back wall. Once it stops, use FrontLeft ultrasonic to detect whether it breached the low bar.

If it did, take the following additional steps:

• Rotate clockwise 120 degrees, such that the left edge is parallel with left-most tower wall.
• Shoot boulder (not dump). Hopefully, score.

Otherwise:

• Rotate clockwise 30 degrees, such that the left edge is parallel with the right-most tower wall.
• Shoot boulder (not dump). Hopefully, score.

(note: If we start by breaching a middle defense, such that either rotation will not line up with a low goal, then the end result will be the same as a "dump boulder". Scoring will only happen for low bar and right-most defense.)

Change the left and right fields to private scope. Generally, fields should not be public, since it is then possible to modify them without the rest of the object being aware of the change. Looking at it another way, the left and right fields are an implementation detail of the class. You should be able to change those fields (e.g. rename them, change their type, etc.) without breaking any code outside of the class.

For Approach, Enter, Exit, and Clear, do not zero the throttle. Let the next sub-activity decide what the throttle should be.

Leaving the scope off of the field means "package-private". "package-private" means that any class in the same package has access. We probably want to restrict the field scope to just subclasses. Note that the "protected" scope would allow a subclass outside of the package to access the field, which is not possible with "package-private". In our case, though, we intend for the class to be subclassed and for that field to be accessed by the subclass, so "protected" is more appropriate here.

Add "right.pidWrite" call.

In general, we want to call currentActivity.update() in each of the periodic methods. In essence, we are simply delegating the periodic update to whatever the current activity is. The one potential issue is that the current activity might not be appropriate for a given mode. For instance, suppose we are in autonomy mode and a breach is currently being performed. FRC switches the robot to teleop. If we do nothing, the robot will continue to perform it's activity until it calls getDefaultActivity(), at which point the activity being returned is the ManualDrive activity. This is likely safe (i.e. no matter what the robot is doing in autonomy mode, it can safely finish its current activity even though it has been switched to teleop mode). However, what should happen if we switch from autonomy or teleop to disabled? In this case, we want to make sure that whatever current activity is active, it should be cancelled. To do this, we would probably call currentActivity.cancel() inside initDisabled().

Which also makes me realize that we could probably provide default implementation of cancel() in Activity that simply calls robot.setActivity(robot.getDefaultActivity()). In many cases, the default behavior will be good enough. In others (e.g. when we've spawned concurrent threads), we will want to override and add some additional cancellation behavior.

Similarly, I'd go ahead and make Activity.initialize() an empty method (not abstract). That way, subclasses that don't need to initialize (like Idle, Autonomy, and ManualDrive) don't need to provide overrident implementations.

The default activity in autonomy mode should be an activity that encapulates our autonomy logic. This logic will then switch the activity to other activities, such as DetectDefenseType, Turn, etc.

I'm not happy with the way I have done it and frankly have no idea how to do it correctly.

Move everything from Arm into ArmController. For the lift() method, you have a couple options:

1. Make it a private method (e.g. _lift()), then in the public lift(), your lambda would look like:

executor.execute( () -> _lift(speed));

1. You can embed the entire code block in the lambda:

executor.execute( () ->
    {
        if(speed < 0 ) {
            if(getAngle() < 1)
                motor.set(speed);
        } else if(speed > 0) {
            if(getAngle() > .05)
                motor.set(speed);
        } else {
            motor.set(0);
        }
    });

Each time we switch to an activity, we want to call the initialize code to reset the activity. Then, when we start calling update(), we know that we are starting the activity in a known state.

Note: For the autonomy activity, we are going to leave initialize() empty! That way, every time that the currentActivity is set back to that object, calls to update() will continue from the last known state. For instance, imagine the following sequence:

1. setActivity(getDefaultActivity()) sets currentActivity to the autonomy activity
2. since this activity is in its initial state, the first thing that update() does is update it's internal state to 'breaching' (don't worry how we implement this yet). Part of that state update causes autonomy to call robot.setActivity(robot.getDetectDefenseActivity()).
3. currentActivity is now DetectDefenseType. Calls to update() go through whatever sequence (always from the beginning, since initialize() had reset the internal state) we program to detect the defense. Assume it determines the defense to be Low Bar. It then calls robot.setActivity(robot.getBreachLowBarActivity()).
4. currentActivity is now BreachLowBar. Calls to update() go through the sequence (always from the beginning, since initialize() had reset the internal state) we program to breach the low bar. Once complete, it calls robot.setActivity(robot.getDefaultActivity()).
5. currentActivity is now back to the autonomy activity. However, since initialize() is empty, the internal state is still at "breaching". Then, when update() is call, it moves onto the next state. For instance, it might be "turning". In which case, it will also call robot.setActivity(robot.getTurnActivity()).
6. currentActivity is now Turn... and so on...