Made the code good

[siIverfish]

I made a whole new class for command trajectory generation and put it in RobotContainer