P2 Prevent Caffeine from taking wrong shortcuts

[Raindancer0912]

Description:

• Currently Caffeine tries to take a straight path to further waypoints, only realizing there are obstacles too late and turning back
• When trying to get from waypoint 4 back to starting position, it tries to cut across the open area in the middle of the track, which is not allowed
• If lane detection fails for a single short segment, it could cause caffeine to try to take impossible shortcuts

Goals:

• Prevent Caffeine from cutting across middle field
• Add in artificial waypoints in the corners so caffeine doesn't try to take shortcuts/decrease dependence on lane detection
• (Maybe) Add a constant box behind caffeine so it doesn't try to turn drastically. Should not interfere with mapping

Acceptance Criteria:

• Caffeine completes the lap along the desired pathway
• Waypoints are added correctly and tuned so it isn't located in a obstacle/out of range