` low p1
pulsout p1, 5
input p1
pulsin p1, 0, toolong1, 40000, wdist1
wdist1 = wdist1/148
serout s_out, i9600, "Distance1: ", sdec wdist1]
toolong1
low p2
pulsout p2, 5
input p2
pulsin p2, 0, toolong2, 40000, wdist2
wdist2 = wdist2 / 148 ;convert for inches
serout s_out, i9600, "Distance2: ", sdec wdist2]
toolong2:
low p3
pulsout p3, 5
input p3
pulsin p3, 0, toolong3, 40000, wdist3
wdist3 = wdist3 / 148 ;convert for inches
serout s_out, i9600, "Distance3: ", sdec wdist3]
toolong3:
low p4
pulsout p4, 5
input p4
pulsin p4, 0, toolong4, 40000, wdist4
wdist4 = wdist4 / 148 ;convert for inches
serout s_out, i9600, "Distance4: ", sdec wdist4]
toolong4:
low p5
pulsout p5, 5
input p5
pulsin p5, 0, toolong5, 40000, wdist5
wdist5 = wdist5 / 148 ;convert for inches
serout s_out, i9600, "Distance5: \", sdec wdist5]
toolong5:
low p6
pulsout p6, 5
input p6
pulsin p6, 0, toolong6, 40000, wdist6
wdist6 = wdist6 / 148 ;convert for inches
serout s_out, i9600, "Distance6: ", sdec wdist6]
toolong6:
low p7
pulsout p7, 5
input p7
pulsin p7, 0, toolong7, 40000, wdist7
wdist7 = wdist7 / 148 ;convert for inches
serout s_out, i9600, "Distance7: ", sdec wdist7]
toolong7:
;===================================================================
;sensor distance
if wdist1 <= 2 then
Steering = 0
XSpeed = 0
YSpeed = -20
Height = 0
LegUpShift = 40
GaitSpeed = 10
KneeShiftOn = 0
LockLegs = 0
LeftStickMode = 0 ;
else
Steering = 0
XSpeed = 20
YSpeed = 0
Height = 0
LegUpShift = 40
GaitSpeed = 30
KneeShiftOn = 0
LockLegs = 0
LeftStickMode = 0
endif