ZJU Robotics II

Path Plan

JPS，写的不是很好，似乎有点bug，不过每次在想去复现bug时都不会出现。平均用时0.01s。

Trajectory Plan

反馈控制法，得调参数，我和另一个组员比较懒，凑合凑合得了，大概把速度调到了2000就懒得调了

静态最好只生成一次路径，可以改改main函数实现

增加倒车控制，修改教材内优化后的控制律

运行速度还行，较其他同学快点，毕竟老师说是竞速。