Flowchart for learning programming, particularly Odometry

I teach robotics and have some of my programming minded students working on building a flowchart for learning odometry in stages rather than all at once. Currently my Jr/Sr. new-to-robotics-but-not-to-C++ students are on step 5 or 6. Here are the tasks I have had my programmers complete to expedite understanding autonomous navigation. Please give me feedback, or let me know if you think there are better checkpoints instead of the ones below.

  1. Make a translational PD control for one motor.

  2. Make a rotational PID with the IMU

  3. Add an element to the rotational PID to compare target angle, target-360, and target+360 to find which results in the lowest error.

  4. sum the rotational and translational values to create a drive straight PID with constant target angle.

  5. Track position by updating an array with [x, y] based off of the instantaneous differential drive, Sideways center ODO wheel, and IMU. Have the path use deltatheta/2 to approximate the new position while driving in a curved path, since it is a good approximation of a constant radius of curvature. Use the documents to complete this.

  6. Identify instantaneous target values for translation and rotation by rotating the target [xf, yf] around the robots coordinates [xi, yi] an angle equal to the robot’s heading. This will then give you [xf’, yf’] and you can simply subtract yf’-yi to assign your translational target value, and then use arctan to assign your rotational target value. This calculation will need to be performed every loop and the target values updated.

You have now entered carrot-on-a-stick mode.

  1. Create a series of waypoints and upon completion either update [xf,yf] or perform tasks. This is basic straight line navigation, and can even handle the robot being pushed around. Just make sure that you have two waypoints per destination to make sure that you have a chance to set and orient the robot.

  2. Use a version of spline, bezier, or some other interpolation to blend your waypoints or set desired curves

  1. Attempt a programming skills using only these splined waypoints and attempt to push the robot off course or introduce barriers. Find weaknesses.

  2. Introduce visual and other sensors to optimize the detection of grasped objects.

  3. Use vision and distance sensors for the final docking of a movement, and use this to recalibrate your global [x, y] based on an assumed position of the object.

  4. Possibly learn how this works on x drives. There are resources available, and at this point you are probably ready to learn how to compute using each of the 4 wheel positions into the x and y calculations.

  5. Use A* or some other search algorithm to take an array representation of the field with obstacles and have the robot pathfind its way from waypoint to waypoint, avoiding obstacles along the way. This will replace your spline interpolation with a better series of waypoints. This will prepare you for VEXAI

Past this is stuff I have no experience with so I can just suppose…

  1. Use the vision sensor to build a simple point cloud of object colors and locations and then start to build an decision making algorithm that evaluates positions, and auto-pathfinds, grabs, and scores or descores as needed.

  2. Have your AI learn to play D.

  3. AI Supremacy.

Here are a few resources I have found along the way.

Github for odometry

Forum Post

Document on matrix transformations


I’d perhaps add a step between 6 and 7 - program the robot to “TurnToPoint/DriveToPoint” thru a series of waypoints, and observe how artificial/non-optimized the movements are.

I’m sure you probably do this already, but I think part of getting folks thru these layers of complexity is for them to discover the limitations of the previous step. So many posts here are “My drive won’t go where I programmed it to go”. Students should be able to decompose that into:

  1. I don’t turn accurately
  2. I don’t drive the right distance
  3. I don’t drive straight.

Perhaps along the way students should understand the impact of unreliable/jittery sensor data can have on their system, perhaps leading to programming filters. The famous image of accuracy and reliability can be helpful:

Along the way, students should discover programming concepts like “Don’t Repeat Yourself”, Separation of Concerns, Abstraction, etc.


Also, presumably there’s discussion in the early stages of how to tune PID systems


I suggest adding a 0 to the entire flowchart: learn cpp. Any step further on is pretty much impossible to do without a good understanding of how c++ works.


a bit after 0 lets say 0.1 is that you should that understand what a PD/ PID loop is


Yeah, that one is big. A lot of new programmers try and copy and paste other people’s PID’s into their program without knowing how the code works, then they get stuck and don’t know what to do.

Thanks! This is a good call, and is what I was fishing for. I just found out that there is some trouble with turning proportions depending on speed.

@Mentor_355v We learn how to make PIDs and tune them in our class/club in a very hands on way, to learn how to tinker, gather evidence, and use buttons + print to screen to tune the pid.

@Ryan_4253B I use SoloLearn for students who are not already proficient in CPP, and it works pretty well. They don’t have to be great to do a lot of this flowchart.


Most of the time these days, if you see a flowchart it’s just something someone scribbled on the whiteboard as a visual aid during a design meeting, or else it’s something that was put together after the code was written in order to help document how things work for someone joining the team or maintaining the code later.

Can you explain how to do step 5 a bit more. What is " instantaneous differential drive"? Why do you need the sideways center ODO wheel and the inertial, why not just inertial? I’ve seen the 5225 document but I doesn;t talk about inertial sensor.

Perhaps you should consider the fact that the inertial sensor was announced after 5225A developed this document


But the 3-wire gyro existed, and its similar to the inertial. However, im not completely sure im a freshman and this is my first year :skull:

The 3-wore was notorious for drift issues. Thus it was better for them to calculate their heading via their encoder wheels instead of through an available sensor

1 Like

How would I do step 8? I’m really confused on how matlab would work with vexcode. I have odometry, I just want curved points

did you watch the video op provided? it should be pretty clearly shown in it

1 Like