void SetPosition(){
unsigned char posit = 1;
if(p3_sw_trig == 1){
posit++;
if (posit == 6){posit =1;}}
if(p4_sw_trig == 1){
posit--;
if(posit == 0){posit = 6}}}

void ArmPosition(int posit, char override){

if(override == 0) {SetPosition()
switch (posit)