I used a geometric identity called incircle to create a region that the foot can reach, so no ‘out of range’ positions are possible. This circle is used in the foot path calculation- each foot location is then sent to the IK routine for angles.
The hex can then walk in any direction
Turning was quite a challange- iI used a routine to find the intersection of the turn circle and the incircle, then plot points withing the incircle.
It uses Xan’s Sin/Cos/ArcCos/ATAN2 routine from the Phonex code (AWESOME!)
For walking, there are 5 down positions and three up positions which yeilds a tripod gate at all speeds.
For collosion avoidance it uses a LVF/moving window to pick the furthest point(s) away- and then steer the hex in that direction- the eye scans back and fourth…
It uses a simple analog joystick for control now.
There are a couple of things not yet implemented, but the code is commented pretty well… I haven’t coded in BASIC for 30 years, so don’t laugh too hard 
The code has not been cleaned up yet, so a couple of extra variables are present
Final note: X is left/right, Y is front/back, and Z is up/down
Thanks to everyone on here for posting- it helped quite a lot!
Here is the code as it stands now:
** updated 3/19/2012 for syntax/comments… -eye function does not work yet**
** updated 3/24/2012 - eye works (sort of), and cleand up routine so re-path of foot occurs only if leg is off the ground- stopped turning right for some reason… I’ll post an update when it’s working…
** updated 3/24/2012 - all works. Real cool. During the video shoot, servo died. ;( Will post anyway 
[code];[Constants]
c1DEC con 10
c2DEC con 100
c4DEC con 10000
c6DEC con 1000000
;--------------------------------------------------------------------
;[TABLES]
;ArcCosinus Table
;Table build in to 3 part to get higher accuracy near cos = 1.
;The biggest error is near cos = 1 and has a biggest value of 3*0.012098rad = 0.521 deg.
;- Cos 0 to 0.9 is done by steps of 0.0079 rad. (1/127)
;- Cos 0.9 to 0.99 is done by steps of 0.0008 rad (0.1/127)
;- Cos 0.99 to 1 is done by step of 0.0002 rad (0.01/64)
;Since the tables are overlapping the full range of 127+127+64 is not necessary. Total bytes: 277
GetACos bytetable 255,254,252,251,250,249,247,246,245,243,242,241,240,238,237,236,234,233,232,231,229,228,227,225, |
224,223,221,220,219,217,216,215,214,212,211,210,208,207,206,204,203,201,200,199,197,196,195,193, |
192,190,189,188,186,185,183,182,181,179,178,176,175,173,172,170,169,167,166,164,163,161,160,158, |
157,155,154,152,150,149,147,146,144,142,141,139,137,135,134,132,130,128,127,125,123,121,119,117, |
115,113,111,109,107,105,103,101,98,96,94,92,89,87,84,81,79,76,73,73,73,72,72,72,71,71,71,70,70, |
70,70,69,69,69,68,68,68,67,67,67,66,66,66,65,65,65,64,64,64,63,63,63,62,62,62,61,61,61,60,60,59, |
59,59,58,58,58,57,57,57,56,56,55,55,55,54,54,53,53,53,52,52,51,51,51,50,50,49,49,48,48,47,47,47, |
46,46,45,45,44,44,43,43,42,42,41,41,40,40,39,39,38,37,37,36,36,35,34,34,33,33,32,31,31,30,29,28, |
28,27,26,25,24,23,23,23,23,22,22,22,22,21,21,21,21,20,20,20,19,19,19,19,18,18,18,17,17,17,17,16, |
16,16,15,15,15,14,14,13,13,13,12,12,11,11,10,10,9,9,8,7,6,6,5,3,0
;--------------------------------------------------------------------
;Sin table 90 deg, precision 0.5 deg (180 values)
GetSin wordtable 0, 87, 174, 261, 348, 436, 523, 610, 697, 784, 871, 958, 1045, 1132, 1218, 1305, 1391, 1478, 1564, |
1650, 1736, 1822, 1908, 1993, 2079, 2164, 2249, 2334, 2419, 2503, 2588, 2672, 2756, 2840, 2923, 3007, |
3090, 3173, 3255, 3338, 3420, 3502, 3583, 3665, 3746, 3826, 3907, 3987, 4067, 4146, 4226, 4305, 4383, |
4461, 4539, 4617, 4694, 4771, 4848, 4924, 4999, 5075, 5150, 5224, 5299, 5372, 5446, 5519, 5591, 5664, |
5735, 5807, 5877, 5948, 6018, 6087, 6156, 6225, 6293, 6360, 6427, 6494, 6560, 6626, 6691, 6755, 6819, |
6883, 6946, 7009, 7071, 7132, 7193, 7253, 7313, 7372, 7431, 7489, 7547, 7604, 7660, 7716, 7771, 7826, |
7880, 7933, 7986, 8038, 8090, 8141, 8191, 8241, 8290, 8338, 8386, 8433, 8480, 8526, 8571, 8616, 8660, |
8703, 8746, 8788, 8829, 8870, 8910, 8949, 8987, 9025, 9063, 9099, 9135, 9170, 9205, 9238, 9271, 9304, |
9335, 9366, 9396, 9426, 9455, 9483, 9510, 9537, 9563, 9588, 9612, 9636, 9659, 9681, 9702, 9723, 9743, |
9762, 9781, 9799, 9816, 9832, 9848, 9862, 9876, 9890, 9902, 9914, 9925, 9935, 9945, 9953, 9961, 9969, |
9975, 9981, 9986, 9990, 9993, 9996, 9998, 9999, 10000
;--------------------------------------------------------------------
;GetSinCos / ArcCos
AngleDeg1 var sword ;Input Angle in degrees, decimals = 1
ABSAngleDeg1 var word ;Absolute value of the Angle in Degrees, decimals = 1
ssin4 var sword ;Output Sinus of the given Angle, decimals = 4
ccos4 var sword ;Output Cosinus of the given Angle, decimals = 4
AngleRad4 var sword ;Output Angle in radials, decimals = 4
NegativeValue var bit ;If the the value is Negative
;--------------------------------------------------------------------
;GetAtan2
AtanX var sword ;Input X
AtanY var sword ;Input Y
Atan4 var sword ;ArcTan2 output
XYhyp2 var long ;Output presenting Hypotenuse of X and Y
;--------------------------------------------------------------------
;--------------------------------------------------------------------
; Use IK to develop foot path movements within the incircle defined by:
; A=(HH pivot poit), B=(Foot position at z height at HH Max), C=(Foot position at z height at HH Min)
; Calculate (x_cen,y_cen)=foot center, incircle radius
;
; Then calculate (5) joint angle positions that place foot through (x_cen,y_cen) with limits at +/- radius
; (3) foot positions for foot UP at AEP (0), Center, and PEP (10)
; Define x = + out to side
; y = + towards front
; z = + up
; Horizontal angles measured counter-clockwise from y=0 from right (+)
; Vertical angles measured counter-clockwise from x,y=0 from horisontal to up (+)
;
;Servo offsets
HH_offset swordtable -1000, -1500, 0, 2000, 1500, 0 ;Servo offsets- actual HSERVO adders, no modifiers
HV_offset swordtable 0, 1500, 0, -2750, 750, -1250 ;leg 0,1,2,3,…
K_offset swordtable 500, -1750, -750, 1500, -1250, -250
;Foot positions
x_foot var sword(48) ;x10 X foot position in sequence (86)=48
y_foot var sword(48) ;x10 Y foot position in sequence (86)=48
z_foot var sword(48) ;x10 Z foot position in sequence (86)=48
;Joint angles
HH_ang var sword(48) ;token angle 30,000 to -30,000 PATH
HV_ang var sword(48) ;token angle 30,000 to -30,000 PATH
K_ang var sword(48) ;token angle 30,000 to -30,000 PATH
;Global lengths
GLength bytetable 12,18,46,12,18,46,12,18,46,12,18,46,12,18,46,12,18,46 ; ( x10)… leg 0 (hip, thight, shin), leg 1 (hip, thigh…
X_Offset sbytetable -10, -16, -10, 10, 16, 10 ;X offset from center of legs 0-5 x10
Y_Offset sbytetable 23, 0, -23, 20, 0, -23 ;Y offset from center of legs 0-5 x10
;Angle limits
GHH_max swordtable 1100, 1600, 2100, 700, 200, -300 ;Left:Leg 0(Front),1(Mid),2(Rear) Right Leg 3(Front),4(Mid),5(Rear)
GHH_min swordtable 1500, 2000, 2500, 300, -200, -700 ;angles conunter-clockwise from right side
HH_max var sword(6) ; degrees x10
HH_mid var sword ; degrees x10
HH_min var sword(6) ; degrees x10
;Leg pin connections
HH_pin ByteTable 3, 22, 19, 0, 6, 16 ;hip horizontal rotate servo
HV_pin ByteTable 4, 23, 20, 1, 7, 17 ;hip vertical rotate servo
K_pin ByteTable 5, 9 , 21, 2, 8, 18 ;knee rotate servo
;Flip servo rotation directions (if needed = -1)
Flip sbytetable 1,-1,1, 1,-1,1, 1,-1,1, -1,1,-1, -1,1,-1, -1,1,-1 ; HH1, HV1, K1, HH2, HV2, K2,…
;Sequence control
seq var word ;sequence value
leg_seq var byte(6) ;sequence value for leg
leg var byte ;Leg number 0-5
leg_idle var word ;leg moving=0, stopped =1
legs_idle var word ;ALL legs stopped =1
repath var byte ;re-calulate path flag
;Joint speed control
HHdist var word ;Token travel
HVDist var word ;Token travel
Kdist var word ;Token travel
Fastest_time var word ;fastest step movement
Fast con 3633 ;fastest servo speed, in tokens/ 10,000 second
HHspeed var word ;HH servo speed
HVspeed var word ;HV servo speed
Kspeed var word ;K servo speed
step_time var word ;sequence step time, seconds x 10,000
;Height variables
height var sbyte(6) ;Z distance from HV joint to foot (-) DOWN x10 (-128 min 127 max -> -12.8 to 12.7)
height_delta var sbyte(6) ; x10 foot lift height (-) DOWN
int_height con -40 ; x10 inital leg height
;Speed and direction control
vector var sword ; angle x 10 from y axis. (-) = left, (+) = right
old_vector var sword
Speed var sbyte ;-127 to 127 (+)forward, (-)reverse
old_speed var sbyte
;Incircle variables
x_HH_max var sword ; x10 foot pos on z plane
y_HH_max var sword ; x10 foot pos on z plane
x_HH_mid var sword ; x10 foot pos on z plane
y_HH_mid var sword ; x10 foot pos on z plane
x_HH_min var sword ; x10 foot pos on z plane
y_HH_min var sword ; x10 foot pos on z plane
x_cen var sword(6) ; x10 incircle center
y_cen var sword(6) ; x10 incircle center
proj_rad var long; Max leg radius in x-y plane at z height
radius var byte(6) ;x10 incircle radius
Len_A var word ; x10
; Len_B not needed by symetery
Len_C var word ; x10
Area var long ; x10
Perimeter var word ; x10
min_radius var byte ;Smallest incircle radius of all 6 legs
;IK variables
sleg var byte ;conversion from seq to leg -> sleg=seq/9
IK_proj_rad var long
IK_psi_angle var sword
IK_theta_angle var sword
IK_cos_K_angle var sword
IK_sin_K_angle var sword
IK_temp_x var sword
IK_temp_y var sword
;Turning
TurnLRC var sbyte ; -128 to 127 turn left/right
Old_turnLRC var sbyte
cen_x_turn var sword ;center of turn point x10 -> max value 3200
cen_y_turn var sword ;center of turn point x10 -> max value 3200
cen_x_in var sword ;center of incircle rel to body center x10
cen_y_in var sword ;center of incircle rel to body center x10
x1_turn var sword ;x10 -> intersection max of incircle and turn radius (body coords)
x2_turn var sword ;x10 -> intersection min of incircle and turn radius(body coords)
y1_turn var sword ;x10 -> intersection max of incircle and turn radius
y2_turn var sword ;x10 -> intersection min of incircle and turn radius
rx_turn var sword ;x10 -> offset from midpiont to circle intersection
ry_turn var sword ;x10 -> offset from midpiont to circle intersection
x3_turn var sword ;x10 -> circle mid-intersection point
y3_turn var sword ;x10 -> circle mid-intersection point
A_turn var word ;x10
H_turn var word ;x10
cen_dist var word ;x10 Distance between incircle center and turn center
;Joystick variables
Joy_speed var word
Joy_speed_center con 445
Joy_db con 50
Joy_shighslope con 417
Joy_shighinter con -115
Joy_slowslope con 302
Joy_slowinter con -127
Joy_speed_pin con 31
;
Joy_turn_pin con 30
Joy_turn var word
Joy_turn_center con 510
;Eye variables
Eye_H swordtable -9335, -8297, -7260, -6223, -5186, -4149, -3112, -2074, -1037, 0, 1037, 2074, 3112, 4149, 5186, 6223, 7260, 8297, 9335 ;Token -> -45,-40,… 0, 5, 10…45; 40/67 factor
Eye_H_pin con 10
Eye_val_pin con 24
;
scan_flag var bit ;1 = scan done
Look_index var byte ;index for eye H table
Look_inc var sbyte ;index direction
window_square var word ;calc square root sum of squares
window_square_max var word ;max square root sum of squares
hole_index var byte ;index position of largest value
Eye_dist_temp var word
Eye_dist var word (19) ;distance value
Eye_dist_last var word(19) ;prevoius distance value
close con 35 ;distance considered “close enough” in centimeters
;Clock
clock var long ;clock value
clock_bit var clock.bit21 ; cycles every 2^21 / 20,000,000 = 0.104 seconds
;Behavior varaibles
Mood var byte ;speed =2((mood/2)-64) -> 1= fast reverse, 128 = stop, 255 = fast forward
dest_x var sword ;someday…
dest_y var sword
Auto con 1 ;1= auto control, 0= joystick
;Other variables
i var byte ; counter
ii var byte ; couter
ileg var byte ;leg sequence counter- DO NOT USE FOR ANY OTHER PURPOSE!
Pi con 31416 ;Pi x 10,000
iconvert con 19099 ; radians to token -> 30,000/(pi/2) = 19099
angle_temp_i var sword ; 0-360 deg * 10
;
; Start Program
;
;^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
;^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
;
Main
;
Hex_start
;
SETHSERIAL1 H9600,H8DATABITS,HNOPARITY,H1STOPBITS ;for terminal window
;
FOR i = 1 to 20 ;give us a few seconds after power up
HIGH P45 ;status light
PAUSE 2500/i
LOW P45
PAUSE 2500/i
NEXT
;
;Initialze servos so hex leg looks like _/-
FOR i = 0 to 5
HSERVO[HH_pin(i)\0+HH_offset(i)] ;move to zero position
PAUSE 500
NEXT
;
FOR i = 0 to 5
HSERVO[HV_pin(i)\200008/15flip(i3+1)+HV_offset(i)] ;move to 60 deg position
PAUSE 500
NEXT
;
FOR i = 0 to 5
HSERVO[K_pin(i)\0+K_offset(i)] ;move to zero position
PAUSE 500
NEXT
;
;If conected…
HSEROUT "VS1 voltage is: ",real (TOFLOAT hservostate 32)/1024.020.0\2,“v”,13]
HSEROUT "VL voltage is: ",real (TOFLOAT hservostate 33)/1024.020.0\2,“v”,13]
;Initialize incircles
FOR leg = 0 to 5
;
HH_max(leg) = GHH_max(leg)
HH_min(leg) = GHH_min(leg)
;
height(leg) = int_height
height_delta(leg) = 10
GOSUB Change_height[leg]
;
GOSUB Incircle[leg]
NEXT
;
GOSUB Get_min_radius
;|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
;Lower hex to initial height and move to tripod gate start
;
FOR leg = 0 to 5
x_foot(leg8) = ABS(x_cen(leg)) ;Hip offset + thigh
y_foot(leg8) = 0
GOSUB IK_foot[leg8] ;seq=0,8,16…
NEXT
;Raise hex to start height
step_time =10000 ;5 sec
FOR leg = 0 to 5
leg_seq(leg)=0 ;just to be sure
GOSUB Leg_movement[leg]
NEXT
;
HSERVOWAIT [HH_pin(5), HV_pin(5), K_pin(5)] ;last leg
;
;**** intitialize some variables *****
vector = 0
turnLRC=0
Old_turnLRC=0
look_index =0
look_inc = 1
scan_flag =0
mood = 128
;Calc paths
FOR leg = 0 to 5
GOSUB Path_down[leg]
GOSUB Path_up[leg]
;
NEXT
;
GOSUB Get_fastest_time ;ALL legs
GOSUB Tripod_gait ;set gait
;
;Set inital positions-> front, rear, center
leg = 0
GOSUB Reset_leg[leg]
leg = 3
GOSUB Reset_leg[leg]
leg = 2
GOSUB Reset_leg[leg]
leg = 5
GOSUB Reset_leg[leg]
leg = 1
GOSUB Reset_leg[leg]
leg = 4
GOSUB Reset_leg[leg]
;|||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
;
;^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
;^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
;
Walking
;Clock
clock =HSERVOTIME 0
;Get desired height
;
;Get speed input, turning input, and strafe vector input
IF Auto THEN
speed =2*((mood/2)-64) ;1= fast reverse, 128 = stop, 255 = fast forward
GOSUB Eye_look ;radius_turn is calc in sub
IF Old_turnLRC <> turnLRC THEN
Old_turnLRC = turnLRC
Repath=1
ENDIF
ELSE
GOSUB Joystick
vector = 0
IF Old_turnLRC <> turnLRC THEN
Old_turnLRC = turnLRC
Repath=1
ENDIF
ENDIF
;
; Max speed: Speed=125, Min speed: Speed=1 per sequence step.
; Speed = distance/time -> Speed = ABS(Pos1-Pos2) x (0.02)/time… 0.02 is req’d HSERVO constant
; Set time to: 0.05s(fastest safe) to 0.2s(slowest)
;
IF ABS(Speed) < 125 THEN ; Set servo step speed as ratio of Speed
Step_time = 2000-12ABS(Speed) ; Step time -> 500 to 1982
ELSE ;Use fastest possible time for same-time arrival of joints
Step_time = fastest_time ;
ENDIF
;
GOSUB Get_All_State
;
IF legs_idle AND(Speed <> 0 OR turnLRC <>0) THEN
;
FOR ileg =0 to 5
;Change z_foot
;
;Check each leg to see if height or step height has changed
IF z_foot(ileg8)<>height(ileg) OR height_delta(ileg)<>(z_foot(ileg8+7)-z_foot(ileg8)) THEN ;use sequence position 0 and 7
;
GOSUB Change_height[ileg]
GOSUB Incircle[ileg]
repath=1
;
ENDIF
;
IF repath = 1 AND leg_seq(ileg)=6 THEN ;only re-path if leg in air
HIGH P44
GOSUB Get_min_radius
;
GOSUB Path_down[ileg]
GOSUB Path_up[ileg]
;
GOSUB Get_fastest_time
;
IF ileg = 5 THEN ;Last leg. Reset repath
repath=0
ENDIF
ENDIF
;
GOSUB Leg_movement [ileg]
;This mess allows cycling of sequence fowards and backwards based on speed vaule AND turing in place
IF (leg_seq(ileg) <7 AND speed>0) OR (leg_seq(ileg)>0 AND speed<0) THEN
leg_seq(ileg) = leg_seq(ileg) + speed/ABS(speed)
ELSEIF leg_seq(ileg)=0 and speed<0
leg_seq(ileg) = 7
ELSEIF leg_seq(ileg) =7 and speed>0
leg_seq(ileg) = 0
ELSEIF speed=0 ;turning in place
IF leg_seq(ileg)<7 THEN
leg_seq(ileg) = leg_seq(ileg) + 1
ELSE
leg_seq(ileg) = 0
ENDIF
ENDIF
;
NEXT
ENDIF
;
GOTO Walking ;lather, rinse, repeat…
;^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
;*******************************************************************************************************
;*******************************************************************************************************
Leg_movement [leg]
;distance from HERE to leg_seq sequence position
HHdist = ABS(HSERVOPOS(HH_pin(leg))-HH_offset(leg)-flip(leg3)(HH_ang(leg8+leg_seq(leg))))
HVdist = ABS(HSERVOPOS(HV_pin(leg))-HV_offset(leg)-flip(leg3+1)(HV_ang(leg8+leg_seq(leg))))
Kdist = ABS(HSERVOPOS(K_pin(leg))-K_offset(leg)-flip(leg3+2)(K_ang(leg8+leg_seq(leg))))
;
HHspeed= 200HHdist/step_time
HVspeed= 200HVdist/step_time ;200 = 10000/50
Kspeed= 200Kdist/step_time
;
HSERVO [HH_pin(leg) \ flip(leg3)HH_ang(leg8+leg_seq(leg))+HH_offset(leg) \ HHspeed, |
HV_pin(leg) \ flip(leg3+1)HV_ang(leg8+leg_seq(leg))+HV_offset(leg) \ HVspeed, |
K_pin(leg) \ flip(leg3+2)K_ang(leg8+leg_seq(leg))+K_offset(leg) \ Kspeed ]
;
RETURN
;******************************************************************************************************
;*******************************************************************************************************
Get_All_State
;get current ACTUAL leg states
;
Legs_idle = HSERVOIDLE(HH_Pin(0)) AND |
HSERVOIDLE(HH_Pin(1)) AND |
HSERVOIDLE(HH_Pin(3)) AND |
HSERVOIDLE(HH_Pin(4)) AND |
HSERVOIDLE(HH_Pin(5)) AND |
HSERVOIDLE(HV_Pin(0)) AND |
HSERVOIDLE(HV_Pin(1)) AND |
HSERVOIDLE(HV_Pin(2)) AND |
HSERVOIDLE(HV_Pin(3)) AND |
HSERVOIDLE(HV_Pin(4)) AND |
HSERVOIDLE(HV_Pin(5)) AND |
HSERVOIDLE(K_Pin(0)) AND |
HSERVOIDLE(K_Pin(1)) AND |
HSERVOIDLE(K_Pin(2)) AND |
HSERVOIDLE(K_Pin(3)) AND |
HSERVOIDLE(K_Pin(4)) AND |
HSERVOIDLE(K_Pin(5))
;
RETURN
;*******************************************************************************************************
;*******************************************************************************************************
Get_min_radius
;Get smallest incircle radius ->step lengths will be the same
min_radius =255 ;reset to high value
;
FOR leg = 0 to 5
IF radius(leg)< min_radius THEN
min_radius = radius(leg)
ENDIF
NEXT
RETURN
;*******************************************************************************************************
;*******************************************************************************************************
Get_Fastest_Time
;
;Find minimum time possible (maximum movement time) for use in servo speed
;for all current foot path positions for ALL legs
fastest_time = 1 ;reset
;
FOR leg = 0 to 5
FOR i = 0 to 6
;
HHdist = ABS(HH_ang(leg8+i)-HH_ang(Leg8+i+1)) ;distance to next sequence position
HVdist = ABS(HV_ang(Leg8+i)-HV_ang(Leg8+i+1))
Kdist = ABS(K_ang(Leg8+i)-K_ang(Leg8+i+1))
;
IF fastest_time < 200HHdist/fast THEN
fastest_time = 200HHdist/fast
ENDIF
IF fastest_time < 200HVdist/fast THEN
fastest_time = 200HVdist/fast
ENDIF
IF fastest_time < 200Kdist/fast THEN
fastest_time = 200Kdist/fast
ENDIF
;
NEXT
;
;Add 7 to 0- lower foot
HHdist = ABS(HH_ang(Leg8+7)-HH_ang(Leg8))
HVdist = ABS(HV_ang(Leg8+7)-HV_ang(Leg8))
Kdist = ABS(K_ang(Leg8+7)-K_ang(Leg8))
;
IF fastest_time < 200HHdist/fast THEN
fastest_time = 200HHdist/fast
ENDIF
IF fastest_time < 200HVdist/fast THEN
fastest_time = 200HVdist/fast
ENDIF
IF fastest_time < 200Kdist/fast THEN
fastest_time = 200Kdist/fast
ENDIF
NEXT
;
RETURN
;*******************************************************************************************************
;*******************************************************************************************************
IK_foot [seq] ; seq= sequence position 0 to 48
;
;Calculate IK angles for x,y,z position at sequence position seq
;Use x_foot(seq), y_foot(seq), z_foor(seq) as coord for calcs
;
sleg = seq/8 ;seq/8 converts 0-48 to 0-5
GOSUB GetAtan2[ABS(x_foot(seq)),y_foot(seq)] ;ABS removes sign-> no calcs for -x values!
;RESULT:
HH_ang(seq) = Atan4 * iconvert /100008/15 ; from radians to token, 8/15 = servo conversion for HS225BB
;
; total leg length for IK calc (remove fixed extension at Hip horz)
;
IK_proj_rad = sqr(x_foot(seq)x_foot(seq)+y_foot(seq)y_foot(seq))-glength(sleg3)
;
;cos(K_angle) = (lenght^2 + height^2 - thigh^2 - shin^2) / 2 x (thigh + shin)
;
IK_cos_K_angle = 10000(IK_proj_radIK_proj_rad+z_foot(seq)z_foot(seq)-Glength(sleg3+1)Glength(sleg3+1) |
-Glength(sleg3+2)Glength(sleg3+2)) / (2Glength(sleg3+1)Glength(sleg3+2))
IK_sin_K_angle = -SQR(100000000-IK_cos_K_angleIK_cos_K_angle) ;sin(a)^2+cos(b)^2 =1 IF a+b = pi/2 (90 deg)
;
GOSUB GetArcCos[IK_cos_K_angle]
; ** NOTE ** Knee angle DOWN 90 deg-> knee “0” perpendicular to thigh, parallel = 30,000
;RESULT:
K_ang(seq) = (30000-AngleRad4iconvert/10000)8/15 ;8/15 = servo conversion for HS225BB
;
GOSUB GetAtan2[IK_proj_rad,z_foot(seq)]
IK_theta_angle = Atan4 ;Radians x 10,000
;
IK_temp_x = Glength(sleg3+1) + IK_cos_K_angle Glength(sleg3+2)/10000
IK_temp_y = IK_sin_K_angle * Glength(sleg3+2)/10000
;
GOSUB GetAtan2[IK_temp_x,IK_temp_y]
IK_psi_angle = Atan4 ;Radians x 10,000
;RESULT:
HV_ang(seq) = (IK_theta_angle-IK_psi_angle)iconvert/100008/15 ;8/15 = servo conversion for HS225BB
;
'HSEROUT [13,"IK Routine “, DEC sleg, “->”, DEC seq,” HH: ", SDEC HH_ang(seq), " HV: ",SDEC HV_ang(seq), " K: ",SDEC K_ang(seq) ]
;
RETURN
;*******************************************************************************************************
;*******************************************************************************************************
Change_height [leg]
;Change z_foot values for leg
;
FOR i = 0 to 4
z_foot(leg8+i) = height(leg) ;foot down ->sequence 0-4,9-13,…
NEXT
;
FOR i = 5 to 7
z_foot(leg8+i) = height(leg) + height_delta(leg)
NEXT
;
RETURN
;*******************************************************************************************************
;*******************************************************************************************************
Incircle[leg]
; Use max and min foot points to calculate incircle circumscribed by triangle
; Find (x,y) coord in z plane for Max, Mid, and Min
;
;proj radius = SQRT(thigh+shin)^2-height^2]+hip -> Leg projected max radius in X-Y plane at (z) height
proj_rad = SQR((GLength(leg3+1)+Glength(leg3+2))(GLength(leg3+1)+Glength(leg3+2)) - height(leg)height(leg))+ GLength(leg3)
;
;Get HH max, HH min, HH mid (x,y) points
GOSUB Getsincos[HH_max(leg)]
x_HH_max = ccos4 * proj_rad/10000 ; x component
y_HH_max = ssin4 * proj_rad/10000 ; y component
;
HH_mid = (HH_max(leg) + HH_min(leg))/2 ; Mid HH angle
GOSUB Getsincos[HH_mid]
x_HH_mid = ccos4 * proj_rad/10000 ; x component
y_HH_mid = ssin4 * proj_rad/10000 ; y component
;
GOSUB Getsincos[HH_min(leg)]
x_HH_min = ccos4 * proj_rad/10000 ; x component
y_HH_min = ssin4 * proj_rad/10000 ; y component
;
;Triangle side A length is distance between max and min points
Len_A = SQR ((x_HH_max-x_HH_min)(x_HH_max-x_HH_min) +(y_HH_max-y_HH_min)(y_HH_max-y_HH_min))
;Triangle side C and B length is leg reach radius
Len_C = proj_rad
;
GOSUB Getsincos[ABS(HH_max(leg) - HH_min(leg))/2];angle between angles
;
Area = Len_A * Len_C * ccos4 / 20000 ; A= Bh/2
;
Perimeter = Len_A + 2Len_C ;Len_B = Len_C
radius(leg) = 2 * Area / Perimeter
;center of incircle is defined as cen(x,y) = vertex(x,y)side length/perimeter
x_cen(leg) = (x_HH_maxLen_C + x_HH_minLen_C)/Perimeter ; Len_B = Len_C
y_cen(leg) = (y_HH_maxLen_C + y_HH_minLen_C)/Perimeter ; Len_B = Len_C
;
RETURN
;***************************************************************************************************
;****************************************************************************************************
Path_up[leg]
;
;Calculate 3 foot positions- cheat and use same (x,y) as foot down positions!
;
;Foot Up AEP
;
x_foot(leg8+7) = x_foot(leg8) ;AEP position
y_foot(leg8+7) = y_foot(leg8)
GOSUB IK_Foot[leg8+7]
;
; Foot Up PEP
;
x_foot(leg8+5) = x_foot(leg8+4) ;PEP position
y_foot(leg8+5) = y_foot(leg8+4)
GOSUB IK_Foot[leg8+5]
;
; Remaining posion is center
;
x_foot(leg8+6) = x_cen(leg)
y_foot(leg8+6) = y_cen(leg)
GOSUB IK_Foot[leg8+6]
;
RETURN
;***************************************************************************************************
;****************************************************************************************************
Path_down[leg]
;
;Calculate 5 foot positions along vector at (X_cen, Y_cen) inside Incircle defined by HH_min and HH_max
; along line at angle vector
IF turnLRC=0 THEN
FOR i = 0 to 4 ;5 positions
;
GOSUB Getsincos[900-vector] ;rotate bt vector
x_foot(leg8+i) = x_cen(leg) + ccos4 * min_radius * (2-i)/20000
y_foot(leg8+i) = y_cen(leg) + ssin4 * min_radius * (2-i)/20000
;
GOSUB IK_Foot[leg8+i]
NEXT
;
ELSE ;Turning!
;
;Turn point os always out to either side. Radius defined by magnitude of turnLRC
;Smooth out changes in turn input radius
;
IF speed=0 THEN ;turn in place
cen_x_turn = 0
cen_y_turn = 0
ELSE
;Get center of turning point -> (-) is left side, (+) is right side…
cen_x_turn = 3(128 - ABS(TurnLRC))ABS(turnLRC)/turnLRC ;(-) is left, (+) is right
cen_y_turn = 0
ENDIF
;
;Calc intersection points of incircle at center(x,y) and radius turn (x,y)
;Incircle radius is already caluclated (first circle)
;
cen_x_in = x_cen(leg)+x_offset(leg) ;incircle center with coords base at body center
cen_y_in = y_cen(leg)+y_offset(leg)
;
;cen_dist to be the distance between incircle center and turn point => second circle radius
cen_dist = SQR((cen_x_turn-cen_x_in)(cen_x_turn-cen_x_in)+(cen_y_turn-cen_y_in)(cen_y_turn-cen_y_in))
;
;a = (r1^2 -r2^2 + d^2) / 2d -> since r2 = d -> a = r1^2 / 2d
;
A_turn = 100min_radiusmin_radius/(2cen_dist) ;x100. ** does not check if cen_dist =0**
;
;H = SQRT(r1^2 - a^2). Check if r1<=a then there is no solution(one circle is within another).
IF min_radius100 <= A_turn THEN
;Oh crap. Well, this sucks. Let’s, um, pick another turn point that’s like real close…
WHILE min_radius100 <= A_turn
;Move turn center away from incircle center, without crossing center
;new value = old value + turn radius * sign - incircle center
cen_x_turn = cen_x_turn + 3*(128 - ABS(TurnLRC))ABS(turnLRC)/turnLRC - cen_x_in
;Same calcs
cen_dist = SQR((cen_x_turn-cen_x_in)(cen_x_turn-cen_x_in)+(cen_y_turn-cen_y_in)(cen_y_turn-cen_y_in))
A_turn = 100min_radiusmin_radius/(2cen_dist)
WEND
;
ENDIF
;
;H = SQRT(r1^2 - a^2)
H_turn = SQR(min_radiusmin_radius10000-A_turnA_turn)
;
;Point between crcles:
;x3 = x1 + (x2-x1) * a/d
;y3 = y1 + (y2-y1) * a/d
;rx = (0-(y2-y1)) * (h/d) NOTE-> signs will flip below from “-(y2-y1)”
;ry = (x2-x1) * (h/d)
;Max intercept: xi1 = x3 + rx, yi1 = y3 + ry
;Min intercept: xi2 = x3 - rx, yi2 = y3 - ry
;
;Flip signs of rx and ry by sign of turnLRC
rx_turn = -(cen_y_turn-cen_y_in)H_turn/(cen_dist100) turnLRC/ABS(turnLRC)
ry_turn = (cen_x_turn-cen_x_in)H_turn/(cen_dist100) turnLRC/ABS(turnLRC)
;
x3_turn = cen_x_in + (cen_x_turn-cen_x_in)A_turn/(cen_dist100)
y3_turn = cen_y_in + (cen_y_turn-cen_y_in)A_turn/(cen_dist100)
;AEP position
x_foot(leg8) = x3_turn + rx_turn-x_offset(leg)
y_foot(leg8) = y3_turn + ry_turn-y_offset(leg)
;PEP position
x_foot(leg8+4) = x3_turn - rx_turn-x_offset(leg)
y_foot(leg8+4) = y3_turn - ry_turn-y_offset(leg)
;
;It would be possible here to rotate foot positions by strafe vector:
;max_vector = SQR((xmax_turn-x_cen(leg))(xmax_turn-x_cen(leg))+(ymax_turn-y_cen(leg))(ymax_turn-y_cen(leg)))
;GOSUB GetAtan2 [xmax_turn, ymax_turn],angle_temp_i
;GOSUB Get_sin_cos[900-vector+angle_temp_i1800/Pi]
;xmax_turn = x_cen(leg) + ccos4 * max_vector /10000
;ymax_turn = y_cen(leg) + ssin4 * max_vector /10000
;
;min_vector = SQR((xmin_turn-x_cen(leg))(xmin_turn-x_cen(leg))+(ymin_turn-y_cen(leg))(ymin_turn-y_cen(leg)))
;GOSUB GetAtan2 [xmin_turn, ymin_turn],angle_temp_i
;GOSUB Get_sin_cos[900-vector+angle_temp_i1800/Pi]
;xmin_turn = x_cen(leg) + ccos4 * min_vector /10000
;ymin_turn = y_cen(leg) + ssin4 * min_vector /10000
;
;
;Center is the incircle center
x_foot(leg8+2) = x_cen(leg)
y_foot(leg8+2) = y_cen(leg)
; Avg value for other points
x_foot(leg8+1) = (x_foot(leg8) + x_cen(leg))/2
y_foot(leg8+1) = (y_foot(leg8) + y_cen(leg))/2
x_foot(leg8+3) = (x_foot(leg8+4) + x_cen(leg))/2
y_foot(leg8+3) = (y_foot(leg8+4) + y_cen(leg))/2
;
;Calc joint angles
'HSEROUT [13, " TurnLRC: ", SDEC turnLRC, " Speed: ", SDEC speed, " H turn: ", DEC h_turn, " A Turn: ", DEC A_turn]
FOR i = 0 to 4
'HSEROUT [13,"Leg “, DEC leg,” (x,y): ", SDEC x_foot(leg8+i), ", ", SDEC y_foot(leg8+i)]
GOSUB IK_foot [leg8+i]
NEXT
'HSEROUT [13]
;
ENDIF
RETURN
;*******************************************************************************************************
;*******************************************************************************************************
Get_Leg_State[leg]
;get current leg state
;
Leg_idle = HSERVOIDLE(HH_Pin(leg)) AND |
HSERVOIDLE(HV_Pin(leg)) AND |
HSERVOIDLE(K_Pin(leg))
;
RETURN
;****************************************************************************************************
;****************************************************************************************************
Tripod_gait
;Place feet in inital positions
;Tripod gait R3 = R1 = L2, R2 = L1 = L3
leg_seq(0) = 0 ;L1
leg_seq(1) = 4 ;L2
leg_seq(2) = 0 ;L3
;
leg_seq(3) = 4 ;R1
leg_seq(4) = 0 ;R2
leg_seq(5) = 4 ;R3
;
RETURN
;****************************************************************************************************
;****************************************************************************************************
Eye_look
;use variation of LVF (Google it) to determine if obstacles in field of vision
;
IF HSERVOIDLE(Eye_H_pin) AND clock_bit THEN
;Read value
Eye_dist_temp = HSERVOSTATE Eye_val_pin
;Calc distance and limit values
;higher voltage = shorter distance. Min distance ~ 15, Max distance =150
IF Eye_dist_temp< 80 THEN ;No object in range
LOW P45 ;Nothing there
Eye_dist(Look_index)=150 ;Max value
ELSE
HIGH P45 ;I see something!
Eye_dist(Look_index)= 12288/Eye_dist_temp ;12288 =1024/560 ->60 is eye constant
ENDIF
'HSEROUT [13,"Eye Routine pin value: ", DEC Eye_dist_temp, " Calc distance: ", DEC Eye_dist(Look_index)]
;
;If end is reched, start going back
IF look_index = 18 THEN
look_inc =-1
scan_flag = 1 ;scan finished!
ELSEIF look_index = 0
look_inc = 1
scan_flag = 1 ;scan finished!
ENDIF
;Index to next position
Look_index = Look_index + look_inc
;Move eye
HSERVO [Eye_H_pin\Eye_H(Look_index)-2750\1400] ;(note: different servo, -2750 is cal number)
;
ENDIF
;Find a hole!
IF scan_flag THEN
;Use moving window sum of squares of three values to find largest value (longest distance)
;First port in storm- uses first largest value closest to center
window_square_max = 0 ;reset
;
FOR i = 10 to 17
;Right side
window_square = SQR(Eye_dist(i-1)Eye_dist(i-1)+Eye_dist(i)Eye_dist(i)+Eye_dist(i+1)Eye_dist(i+1))
'HSEROUT [13,"i ", DEC i, “->”, DEC window_square]
IF window_square_max < window_square THEN
window_square_max = window_square
hole_index = i
ENDIF
;Left side
window_square = SQR(Eye_dist(19-i)Eye_dist(19-i)+Eye_dist(17-i)Eye_dist(17-i)+Eye_dist(18-i)Eye_dist(18-i))
'HSEROUT [13,"i ", DEC 17-i, “->”, DEC window_square]
IF window_square_max < window_square THEN
window_square_max = window_square
hole_index = 18-i
ENDIF
NEXT
;
;Center
window_square = SQR(Eye_dist(8)Eye_dist(8)+Eye_dist(9)Eye_dist(9)+Eye_dist(10)Eye_dist(10))
'HSEROUT [13,“cen ->”, DEC window_square]
IF window_square_max <= window_square THEN ;<= is important! Pick center if equal
window_square_max = window_square
hole_index = 9
ENDIF
;
scan_flag =0 ;reset for new values
;
IF Eye_dist(7)<close OR Eye_dist(9)<close OR Eye_dist(11)<close THEN ;If close in front then stop and turn
;mood-> 1= fast reverse, 128 = stop, 255 = fast forward
mood = 128 ;STOP
IF hole_index = 9 THEN ;stuck in a corner!
TurnLRC = clock_bit(-100)+50 ;turn random left or right->clock_bit = 1 or 0
ELSE
TurnLRC = Eye_H(hole_index)127/9335
ENDIF
ELSE ;Roam if you wan to!! Roam around the world!!
;Speed ~ distance
mood = 128+(Eye_dist(8)+Eye_dist(9)+Eye_dist(10))/(4) ;largest distance =150-> mood = 128+(313/4) to 128+(1503/4)
;Convert to turn.
TurnLRC = Eye_H(hole_index)127/9335
ENDIF
;
'HSEROUT [13,"Mood: ", DEC mood, "-> ", SDEC 2((mood/2)-64), " TurnLRC: ", SDEC TurnLRC]
ENDIF
;
RETURN
;********************************************************************************************
;****************************************************************************************************
Joystick
Joy_speed = HSERVOSTATE Joy_speed_pin
;
IF Joy_speed > Joy_speed_center+Joy_db/2 THEN
Speed = 100joy_speed/Joy_shighslope + Joy_shighinter
ELSEIF Joy_speed < Joy_speed_center - Joy_db/2
Speed = 100Joy_speed/Joy_slowslope + Joy_slowinter
ELSE
Speed = 0
ENDIF
;
Joy_turn =HSERVOSTATE Joy_turn_pin
;
IF Joy_turn > Joy_turn_center+Joy_db THEN
turnLRC = 100joy_turn/Joy_shighslope + Joy_shighinter
ELSEIF Joy_turn < Joy_turn_center - Joy_db
turnLRC = 100Joy_turn/Joy_slowslope + Joy_slowinter
ELSE
turnLRC = 0
ENDIF
;
RETURN
;*******************************************************************************************************
;*******************************************************************************************************
Reset_Leg [leg]
;Resets leg to current sequence position by lifting leg, then placing leg in sequence position
; ** BLOCKING FUNCTION **
ii = leg_seq(leg) ;Store sequence
leg_seq(leg) = 6 ;Leg up center position
step_time = fastest_time5 ;Fast-ish movement
GOSUB Leg_movement[leg]
HSERVOWAIT[HH_pin(leg), HV_Pin(leg), K_pin(leg)] ;Go ahead, I’ll wait
;
leg_seq(leg) = ii ;Move leg to original sequence
GOSUB Leg_movement[leg]
HSERVOWAIT[HH_pin(leg), HV_Pin(leg), K_pin(leg)] ;Go ahead, I’ll wait, AGAIN
;
RETURN
;***************************************************************************************************
;****************************************************************************************************
;[GETSINCOS] Get the sinus and cosinus from the angle +/- multiple circles
;AngleDeg1 - Input Angle in degrees
;Sin4 - Output Sinus of AngleDeg
;Cos4 - Output Cosinus of AngleDeg
GetSinCos[AngleDeg1]
;Get the absolute value of AngleDeg
IF AngleDeg1 < 0 THEN
ABSAngleDeg1 = AngleDeg1 *-1
ELSE
ABSAngleDeg1 = AngleDeg1
ENDIF
;Shift rotation to a full circle of 360 deg -> AngleDeg // 360
IF AngleDeg1 < 0 THEN ;Negative values
AngleDeg1 = 3600-(ABSAngleDeg1-(3600*(ABSAngleDeg1/3600)))
ELSE ;Positive values
AngleDeg1 = ABSAngleDeg1-(3600*(ABSAngleDeg1/3600))
ENDIF
IF (AngleDeg1>=0 AND AngleDeg1<=900) THEN ; 0 to 90 deg
SSin4 = GetSin(AngleDeg1/5) ; 5 is the presision (0.5) of the table
CCos4 = GetSin((900-(AngleDeg1))/5)
ELSEIF (AngleDeg1>900 AND AngleDeg1<=1800) ; 90 to 180 deg
SSin4 = GetSin((900-(AngleDeg1-900))/5) ; 5 is the presision (0.5) of the table
CCos4 = -GetSin((AngleDeg1-900)/5)
ELSEIF (AngleDeg1>1800 AND AngleDeg1<=2700) ; 180 to 270 deg
SSin4 = -GetSin((AngleDeg1-1800)/5) ; 5 is the presision (0.5) of the table
CCos4 = -GetSin((2700-AngleDeg1)/5)
ELSEIF (AngleDeg1>2700 AND AngleDeg1<=3600) ; 270 to 360 deg
SSin4 = -GetSin((3600-AngleDeg1)/5) ; 5 is the presision (0.5) of the table
CCos4 = GetSin((AngleDeg1-2700)/5)
ENDIF
return
;--------------------------------------------------------------------
;[GETARCCOS] Get the sinus and cosinus from the angle +/- multiple circles
;Cos4 - Input Cosinus
;AngleRad4 - Output Angle in AngleRad4
GetArcCos[CCos4]
;Check for negative value
IF (CCos4<0) THEN
CCos4 = -CCos4
NegativeValue = 1
ELSE
NegativeValue = 0
ENDIF
;Limit Cos4 to his maximal value
CCos4 = (CCos4 max c4DEC)
IF (CCos4>=0 AND CCos4<9000) THEN
AngleRad4 = GetACos(CCos4/79) ;79=table resolution (1/127)
AngleRad4 = AngleRad4*616/c1DEC ;616=acos resolution (pi/2/255)
ELSEIF (CCos4>=9000 AND CCos4<9900)
AngleRad4 = GetACos((CCos4-9000)/8+114) ;8=table resolution (0.1/127), 114 start address 2nd bytetable range
AngleRad4 = AngleRad4*616/c1DEC ;616=acos resolution (pi/2/255)
ELSEIF (CCos4>=9900 AND CCos4<=10000)
AngleRad4 = GetACos((CCos4-9900)/2+227) ;2=table resolution (0.01/64), 227 start address 3rd bytetable range
AngleRad4 = AngleRad4*616/c1DEC ;616=acos resolution (pi/2/255)
ENDIF
;Add negative sign
IF NegativeValue THEN
AngleRad4 = 31416 - AngleRad4
ENDIF
return AngleRad4
;--------------------------------------------------------------------
;[GETATAN2] Simplyfied ArcTan2 function based on fixed point ArcCos
;ArcTanX - Input X
;ArcTanY - Input Y
;ArcTan4 - Output ARCTAN2(X/Y)
;XYhyp2 - Output presenting Hypotenuse of X and Y
GetAtan2 [AtanX, AtanY]
XYhyp2 = SQR ((AtanXAtanXc4DEC) + (AtanYAtanYc4DEC)); CHANGED XYhyp2 to LONG variable
GOSUB GetArcCos [AtanX*c6DEC / XYHyp2]
Atan4 = AngleRad4 * (AtanY/ABS(AtanY)) ;Add sign
return Atan4
;-------------------------------------------------------------------------------------------------------
[/code]
