200 Hz Control

Need to fix:
- Pid gains
- Serial communication