Finished Kalman, PID and started motors.