*SPOILER ALERT* Here comes the code.

Now that this problem is no longer a ZR requirement / challenge, it is time to complete the program. I will be finishing this tutorial with an explanation of how to fly a smooth error correcting orbit around the origin, given an arbitrary axis of rotation, or how to complete the Circle Practice Game. This problem/solution follows the approach used by y0b0tics! to fly a “perfect” orbit around Opulens with the one exception that the competition code could fly around any point in space, not just the origin. (BTW – nothing is perfect, but if you fly well enough to get a perfect score, it is perfect enough – something to remember in ZR competitions.)

The Circle Practice Game is easy. We just want to orbit the origin. This means that every second of the game we want to use our current position to calculate where we want to be next and then get there. That translates to getting our current state, doing some math and then using the ZR API to get there. Yup, that simple. The main thing we really need to work out now is the math. Let’s consider that clue from the last section again:

Now there are a lot of vectors there, but if we start with our “givens” and then march our way through one at a time it will be simple.

The givens are as follows:

• Blue – Axis of Rotation

• Light Green – Actual Position

Let’s also agree on two constants that you can choose:

• R = radius of orbit

• RTanTheta = R times the tangent of the angle we want to orbit each second. We’ll use this later at which point it will make a little more sense. For now think of this as selecting the number of degrees we want to orbit each second and then performing a couple of calculations now so we don’t have to do it later in our program.

Here we go. Notice how the Light Green vector (our actual position) doesn’t lie in the plane of the orbit. We should be in that plane but in the real world maybe we’re off a little. This is something we want to correct for, so let’s calculate where we would be if we were in the right place. This corrected position is represented by the Dark Green vector. To calculate the corrected position we want to “square up” our actual position to the axis of rotation so that our “ideal position” is perpendicular to the axis of rotation and aligned with the desired center of our orbit. In order to do this we’ll use a temporary helper vector represented in the diagram by the Pink vector. The Pink vector is perpendicular to both the axis of rotation and our position. 

Pink = Blue cross Light Green (Pink = the cross product of the Blue and Light Green vectors)

We can now use the Pink vector to find a vector perpendicular to it and the axis of rotation which will provide us with our idealized location (where we really should be now.) 

Dark Green = Pink cross Blue

We can clean up the Dark Green vector up by scaling it to length R (the desired radius of the orbit.)

The Red vector is simply a scaled copy of the Pink vector. We want to scale this to some appropriate length that will give us an appropriate rate of rotation along our orbit. This length is equal to Rtan(θ). This value will not be exact in real life. We can start with it though and tweak it as needed to account for the SPHERES control dynamics.

Our final target (or where we want to be in 1 second) is represented by the Orange vector. The Orange vector is simply (Dark Green + Red) scaled to R.

That is just about it! We still need to figure out how to get there. The simplest way to do this is by using setPositionTarget(Orange). So let’s do that and see what we get…

And here is the code.

void init(){
}
 
void loop(){
    float MyState[12];  // MyState to get Actual Position
    float Blue[] = {1,0,0}; // Axis of Rotation
    float Pink[3];      // Helper
    float DarkGreen[3]; // Ideal Position
    float Red[3];       // Orbital Distance Vector
    float Orange[3];    // Desired Position 1 sec. from now
    float R = .35;
    float RTanTheta = 0.0245; // R*tan(4 degrees)
  
    // So where are we?
    api.getMyZRState(MyState);
     
    // Do the Math
    mathVecCross(Pink,Blue,MyState); // MyState = &MyState[0] = Light Green
    mathVecCross(DarkGreen,Pink,Blue);
    mathVecScale(DarkGreen,DarkGreen,R,true);
    mathVecScale(Red,Pink,RTanTheta,true);
    mathVecAdd(Orange,DarkGreen,Red,3);
    mathVecScale(Orange,Orange,R,true);
     
    // Go there...
    api.setPositionTarget(Orange);
  
}
                          
// To ease scaling use our utility function
void mathVecScale(float res[3], float src[3], float mag, bool norm)
{
    memcpy(res,src,sizeof(float)*3);
    if(norm) mathVecNormalize(res,3);
    res[0]*=mag;
    res[1]*=mag;
    res[2]*=mag;
}

Was that as disappointing for you as it was for me? While the orbit should be very accurate, that was slow even for a SPHERES satellite. The problem was that while setPositionTarget() is good for getting to a specific spot, it is not easy to say “and get there in 1 second.” It has it’s own way about it and is more focused on hitting the spot and stopping in a controlled way than getting there fast. So let’s try the next best thing setVelocityTarget() this way we can tell the satellite what velocity we want to be going at. To calculate that we need to figure out our desired change in position.

Velocity = Orange – MyState (or where we want to be 1 second from now minus where we are now)

Let’s create a new vector Velocity, add the calculation for it and then change setPositionTarget() to be setVelocityTarget().

The code should now look like this…

void init(){
}
 
void loop(){
    float MyState[12];  // MyState to get Actual Position
    float Blue[] = {1,0,0}; // Axis of Rotation
    float Pink[3];      // Helper
    float DarkGreen[3]; // Ideal Position
    float Red[3];       // Orbital Distance Vector
    float Orange[3];    // Desired Position 1 sec. from now
    float Velocity[3];  // Desired Velocity
    float R = .35;
    float RTanTheta = 0.0245; // R*tan(4 degrees)
  
    // So where are we?
    api.getMyZRState(MyState);
     
    // Do the Math
    mathVecCross(Pink,Blue,MyState); // MyState = &MyState[0] = Light Green
    mathVecCross(DarkGreen,Pink,Blue);
    mathVecScale(DarkGreen,DarkGreen,R,true);
    mathVecScale(Red,Pink,RTanTheta,true);
    mathVecAdd(Orange,DarkGreen,Red,3);
    mathVecScale(Orange,Orange,R,true);
    mathVecSubtract(Velocity,Orange,MyState,3);
     
    // Go there...
    //api.setPositionTarget(Orange);
    api.setVelocityTarget(Velocity);
  
}
                          
// To ease scaling use our utility function
void mathVecScale(float res[3], float src[3], float mag, bool norm)
{
    memcpy(res,src,sizeof(float)*3);
    if(norm) mathVecNormalize(res,3);
    res[0]*=mag;
    res[1]*=mag;
    res[2]*=mag;
}

That was a whole lot better. Not that 4° is the Indy Raceway or anything but that looks like it worked. We are still showing a little effect of the SPHERES control dynamics “smoothing” our velocity correction. In the real world (i.e. a ZR competition,) this may not really be a bad thing. It will dampen errors and noise in the system. But since we’ve come this far let’s finish with a setForces() implementation. setForces() is slightly different from the “Target” functions in that it does not smooth the controls you feed it. This can be an advantage at times and a disadvantage at others. It is good to be familiar with both types of control. Just as velocity is the difference (differential) of position, acceleration is the difference (differential) of velocity and of course f=ma. Let’s add two new vectors Accel and Force, then we’ll calculate their values and in the end use a setForces() call to move the satellite. 

The code should now look like this…

void init(){
}
 
void loop(){
    float MyState[12];  // MyState to get Actual Position
    float Blue[] = {1,0,0}; // Axis of Rotation
    float Pink[3];      // Helper
    float DarkGreen[3]; // Ideal Position
    float Red[3];       // Orbital Distance Vector
    float Orange[3];    // Desired Position 1 sec. from now
    float Velocity[3];  // Desired Velocity
    float Accel[3];     // Desired Acceleration
    float Force[3];     // Desired Force
    float R = .35;
    float RTanTheta = 0.0245; // R*tan(4 degrees)
  
    // So where are we?
    api.getMyZRState(MyState);
     
    // Do the Math
    mathVecCross(Pink,Blue,MyState); // MyState = &MyState[0] = Light Green
    mathVecCross(DarkGreen,Pink,Blue);
    mathVecScale(DarkGreen,DarkGreen,R,true);
    mathVecScale(Red,Pink,RTanTheta,true);
    mathVecAdd(Orange,DarkGreen,Red,3);
    mathVecScale(Orange,Orange,R,true);
    mathVecSubtract(Velocity,Orange,MyState,3);
    mathVecSubtract(Accel,Velocity,&MyState[3],3);
    mathVecScale(Force,Accel,4.45,false);
 
    // Go there...
    //api.setPositionTarget(Orange);
    //api.setVelocityTarget(Velocity);
    api.setForces(Force);
  
}
                          
// To ease scaling use our utility function
void mathVecScale(float res[3], float src[3], float mag, bool norm)
{
    memcpy(res,src,sizeof(float)*3);
    if(norm) mathVecNormalize(res,3);
    res[0]*=mag;
    res[1]*=mag;
    res[2]*=mag;
}

That should have been really pretty nice as far as orbits go. I tend to see the satellite run a little slowly, certainly on the first lap where it needs to get up to speed, but a nice orbit just the same. There are endless ways that this can be improved, but one thing to remember is that code space is a major concern and you will likely find that “good enough” can turn into “fantastic” and “just plain okay” may be what you actually compete with, not because you don’t want to do better, but because you can’t afford the code.  

Of course you can solve this problem other ways as well. If you want to experience why I suggest this method, try changing the axis of rotation to {1,.2,0}. This should work just fine despite the fact that you start your orbit out of position. You can also try starting at {0,.5,0}. You should try this with both the setForces() and setVelocityTarget() versions of this program. Which do you like better? Why? Now consider what happens to other solution methods in order to achieve these same results using an arbitrary axis of rotation and not starting on the orbital plane.