September 8, 2016

The Circle Tutorial

Are we there yet?

A classic problem in ZR is determining when you have arrived at a specific location. In an idealized world you could simply compare the elements of your position vector with the desired target vector, but in the real world (and in the ZR simulator) this generally just won’t work because of noise in the system. Your values won’t be exact matches. So we need to rephrase the question and ask “Are we close enough yet?” In order to do this we can simply compare our distance from our target with a “close enough” value. You should now create a new function isCloseEnough(). Insert the following code on the Utilities page.

bool isCloseEnough(float *MyState, float *Target, float Margin)
{
    bool result = false;
    float diff[3];
    mathVecSubtract(diff, MyState, Target,3);
    float dist = mathVecMagnitude(diff,3);
    if(dist<Margin)
        result = true;
     
    return result;
}

You should make sure that you understand this code before proceeding. To demonstrate it nicely, we’ll change our program to fly to the other satellite (at least close enough to the other satellite) and then fly off to a second waypoint. This will require several changes to the rest of the current program. To get back to the main code page click on “main” in the outline view at the left. Then create a new class member bool variable “reachedOther”. This will be used to remember if we’ve completed the first step of our mission – reaching the other satellite. We can’t define it inside loop() because it needs to survive from one call to loop() to the next. Just like our “time” variable we will need to initialize it in init(). The beginning of the program should now look like this.

int time;
bool reachedOther;
 
void init(){
    time = 0;
    reachedOther = false;
}

Now we can use our isCloseEnough function. Inside loop directly after the two lines that get MyState and OtherState insert the following code:

if(isCloseEnough(MyState, OtherState, .1))
{
    reachedOther = true;
}

This code will check every second and if we are close enough it will set reachedOther to true. Once this flag is turned on (set to true) there is nothing in our program that will turn that flag off (set it to false) again. Now we need to check this flag to decide what to do. If the flag is not set we want to run our existing code to travel to the other satellite. If the flag is set we will simply use a setPositionTarget to point to the new location. We now need to wrap the current movement code in an “if” statement and add an “else” clause to move to the second waypoint. This is how loop() should look now:

void loop(){
    DEBUG(("\nHello Space! Time: %d",time));
    float Diff[3];
    float MyState[12];
    float OtherState[12];
     
    api.getMyZRState(MyState);
    api.getOtherZRState(OtherState);
 
    if(isCloseEnough(MyState, OtherState, .1))
    {
        reachedOther = true;
    }
     
    if(!reachedOther)
    {
        mathVecSubtract(Diff,OtherState, MyState,3);
        Diff[0]*=.25f;
        Diff[1]*=.25f;
        Diff[2]*=.25f;
        api.setVelocityTarget(Diff);
    }
    else
    {
        float Waypoint2[] = {0.0f,-0.5f,-0.5f};
        api.setPositionTarget(Waypoint2);
    }
     
    time++;
}

Let’s make our last change by replacing the three “*=” lines with mathVecScale(). Since we want to scale the vector Diff in-place, we will use it for both the result and the source parameters and since we want the result to be scaled by .25, instead of a vector of length .25, we will set the norm parameter to false. Delete the “*=” lines and insert the following line of code in their place.

mathVecScale(Diff,Diff,0.25f,false);

When you run this program you should see the blue satellite fly to the other satellite overshooting as usual, but instead of oscillating back, it should then fly off to its next destination. Play with the margin some and see what happens if you make it much larger (i.e. .6). Once you are comfortable with what is happening here, proceed to the next section.