IT FINALLY WORKS!! SUPER EPIC!! We still need to tune the PID when we test it at Sussex but it works right now <333