CON _xinfreq = 5_000_000 _clkmode = xtal1 + pll16x SERVO_COUNT = 5 VAR long Pause, Pulsout byte pin[SERVO_COUNT] ' Array of servo I/O pins long servo[SERVO_COUNT] ' Array of servo pulse duraitons long pulseCountAddr, pulseCount ' The servo pulse count long temp, buttons, lv, lh, rv, rh, statarm long vspeed long speedlock long stack[1000] long aspeed ' Arm Speed Var long dtSLU ' Drive Train Speedlock Up long dtSLD ' Drive Train Speedlock Down long armSLU ' Arm Speedlock Up long armSLD ' Arm Speedlock Down long gripLock ' Allows gripper direciton control long GLU, GLD ' Safety variables for easy transition. OBJ xbee : "XBee_Object" pst : "Parallax Serial Terminal" Servos : "Servos_PEKITREV" PUB button_control | t, dt, index repeat index from 0 to (SERVO_COUNT - 1) 'Initialize servo signals pin[index] := lookupz(index: 23,24,19,20,22) 'Initialize servo pins servo[index] := lookupz(index: 0,0,0,0,0) 'Empty index for servos xbee.start(4, 3, 0, 9600) 'Starts the servo object. pst.start(9600) 'Starts debugging terminal pst.Home 'All PST methods are not required for normal operation. buttons := 0 'Initialize button variable t := cnt 'time shorthand dT := clkfreq/2 'time shorthand pulseCountAddr := servos.start(@pin[0], @servo[0], SERVO_COUNT) repeat longfill(@temp, 0, 6) repeat until xbee.rx == "!" ' clear temp..RH repeat index from 3 to 5 ' Get buttons temp := xbee.rx temp <<= (index * 8) buttons += temp pst.Clear pst.Str(String("Holding for command")) pst.NewLine if xbee.rx == 51 pst.Str(String("Forward")) servo[0] := 255 elseif xbee.rx == 53 pst.Str(String(" Backward")) servo[0] := 0 else{{ xbee.rx == 53}} pst.Str(String(" S0 Off")) servo[0] := 128 pst.NewLine if xbee.rx == 61 pst.Str(String("Forward")) servo[1] := 255 elseif xbee.rx == 63 pst.Str(String(" BackWard")) servo[1] := 0 else pst.Str(String(" S1 OFF ")) servo[1] := 128 pst.NewLine if buttons & |< 0 pst.Str(String("Hellooooo")) servo[2]:= 255 elseifnot buttons == 1 servo[2] := 128 waitcnt(clkfreq/40 + cnt)