Added Truss Setpoint to constants and added new auto mode: drive forward