pathing_plan_task.py File Reference

Pathing plan task for cooperative scheduler. More...

Go to the source code of this file.

Classes
class	pathing_plan_task.pathing_plan
	Pathing plan task class for ROMI navigation. More...

Namespaces
namespace	pathing_plan_task

Variables
float	pathing_plan_task.PI = 3.1415
	Pi constant for angle conversions.
tuple	pathing_plan_task.YAWGOAL1 = (82)*(PI/180)
	Fork angle.
tuple	pathing_plan_task.YAWGOAL2 = (329)*(PI/180)
	CP1 to CP2.
tuple	pathing_plan_task.YAWGOAL3 = (90)*(PI/180)
	CP2 to Cup.
tuple	pathing_plan_task.YAWGOAL4 = (166)*(PI/180)
	CP3 to CP4.
tuple	pathing_plan_task.YAWGOAL5 = (163.5)*(PI/180)
	CP4 to garage.
tuple	pathing_plan_task.YAWGOAL6 = (255)*(PI/180)
	Garage to CP5.
tuple	pathing_plan_task.YAWGOAL7 = (345)*(PI/180)
	Wall to Cup.
tuple	pathing_plan_task.YAWGOAL8 = (240)*(PI/180)
	Cup to Finish.
int	pathing_plan_task.LINE_FOLLOW_STATE_0 = 0
	Initial line following state.
int	pathing_plan_task.CENTROID_OFFSET_STATE = 1
	Centroid offset adjustment state.
int	pathing_plan_task.ALIGNMENT_GOAL_1 = 2
	First alignment goal state.
int	pathing_plan_task.ALIGNMENT_CONTROL_1 = 3
	First alignment control state.
int	pathing_plan_task.FORK = 4
	Fork navigation state.
int	pathing_plan_task.ALIGNMENT_GOAL_2 = 5
	Second alignment goal state.
int	pathing_plan_task.ALIGNMENT_CONTROL_2 = 6
	Second alignment control state.
int	pathing_plan_task.FREE_TRAVERSE_1 = 7
	First free traverse state.
int	pathing_plan_task.CP2_PIVOT_1 = 29
	CP2 first pivot state.
int	pathing_plan_task.CP2_PIVOT_2 = 31
	CP2 second pivot state.
int	pathing_plan_task.LINE_FOLLOWING_2 = 8
	Second line following state.
int	pathing_plan_task.ALIGNMENT_GOAL_3 = 9
	Third alignment goal state.
int	pathing_plan_task.ALIGNMENT_CONTROL_3 = 10
	Third alignment control state.
int	pathing_plan_task.FREE_TRAVERSE_2 = 11
	Second free traverse state.
int	pathing_plan_task.LINE_FOLLOW_3 = 12
	Third line following state.
int	pathing_plan_task.ALIGNMENT_GOAL_4 = 13
	Fourth alignment goal state.
int	pathing_plan_task.ALIGNMENT_CONTROL_4 = 14
	Fourth alignment control state.
int	pathing_plan_task.FREE_TRAVERSE_3 = 15
	Third free traverse state.
int	pathing_plan_task.LINE_FOLLOW_GRG = 32
	Garage line following state.
int	pathing_plan_task.ALIGNMENT_GOAL_5 = 16
	Fifth alignment goal state.
int	pathing_plan_task.ALIGNMENT_CONTROL_5 = 17
	Fifth alignment control state.
int	pathing_plan_task.FREE_TRAVERSE_4 = 18
	Fourth free traverse state.
int	pathing_plan_task.ALIGNMENT_GOAL_6 = 19
	Sixth alignment goal state.
int	pathing_plan_task.ALIGNMENT_CONTROL_6 = 20
	Sixth alignment control state.
int	pathing_plan_task.FREE_TRAVERSE_5 = 21
	Fifth free traverse state.
int	pathing_plan_task.WALL = 22
	Wall detection state.
int	pathing_plan_task.ALIGNMENT_GOAL_7 = 23
	Seventh alignment goal state.
int	pathing_plan_task.ALIGNMENT_CONTROL_7 = 24
	Seventh alignment control state.
int	pathing_plan_task.CUP = 25
	Cup navigation state.
int	pathing_plan_task.ALIGNMENT_GOAL_8 = 26
	Eighth alignment goal state.
int	pathing_plan_task.ALIGNMENT_CONTROL_8 = 27
	Eighth alignment control state.
int	pathing_plan_task.FINISH_LINE = 33
	Finish line state.
int	pathing_plan_task.FINISH = 28
	Finish state.
int	pathing_plan_task.STOP = 40
	Stop state.
int	pathing_plan_task.TESTING = 30
	Testing state.
int	pathing_plan_task.CP1_2_DISTANCE = 1800
	Distance from CP1 to CP2.
int	pathing_plan_task.LF2_DIST = 2025
	Line follow 2 distance.
int	pathing_plan_task.CUP_CP3_DIST = 2725
	Distance from cup to CP3.
int	pathing_plan_task.CP3_CP4 = 3150
	Distance from CP3 to CP4.
int	pathing_plan_task.CP4_LINE = 3275
	Distance to CP4 line.
int	pathing_plan_task.CP4_GARAGE = 3600
	Distance from CP4 to garage.
int	pathing_plan_task.OUT_GARAGE = 3900
	Distance out of garage.
int	pathing_plan_task.BACK_UP_DIST = OUT_GARAGE + 100
	Backup distance.
int	pathing_plan_task.TO_CUP = 4300
	Distance to cup.
int	pathing_plan_task.RUN_TO_LINE = 4500
	Distance to run to line.
int	pathing_plan_task.FINISH_DIST = 5000
	Finish distance.

Detailed Description

Pathing plan task for cooperative scheduler.

This file contains a class to create a pathing_plan_task object which is compatible with the cotask scheduler to run as a cooperative task.

The pathing_plan object uses a finite state machine design to dictate the motion of the Pololu ROMI kit used in ME 405, Fall 2025 to follow the term project course. The task sets share flags and goals to order how control task moves the ROMI to reach the 6 checkpoints and the two bonus cups on the course. A long list of defines are used at the beginning to make tuning path simpler. The state names are used to make adding new states inbetween states easier so that the file reads in the order of how ROMI will move. The goal defines are what are tuned for the course. When moving forward the task uses the total distance travelled as found by the observer to know when it needs to change states. When turning, the task waits for the yaw from the IMU to be within an acceptable range to change states.

Note

The intention of the angle goals was to be done in a global sense where 0 is the direction ROMI faces at the start. However, the IMU has a lot of drift as ROMI goes through the course so these values are found experimentally and start to lose meaning when read in the code until you see how ROMI actually moves.

Yaw drift is inconsistent so this path is not consistent do not run ROMI unsupervised.

Author

Alex Power, Lucas Heuchert, Erik Heuchert

ChatGPT5 was used for debugging purposes but not to generate this code

Date

2025-Nov-10 Approximate date of creation of file

2025-Dec-12 Final alterations made

Definition in file pathing_plan_task.py.