Most shooter logic done

Got most of the shooter control logic written. needs testing. working on
integration with intake for smart positioning.
finished intake fsm

finished intake fsm and logic