Putting robotics at your service™

Free shipping on orders over $200

Hexapod HS225BB 3DOF legs, ARC32, custom code (video link)

Print view Share :
Previous topicNext topic

Page 1 of 1 [ 11 posts ]

Rookie ( offline )
Posts: 11
Posted: 2012-03-13 21:56 
** scroll down for new video! **

Hi all,

Well after posting here a couple of times, I figured I show what I've been up to for those interested.

Using HS225BB micro servos and an ARC32 controller, I was able to to finally get the hex to walk/rotate.

I created my own code (I'm working on a paper now for it-no, I'm not in school LOL) and plan to have it be autonomus (sp?) using a Sharp range finder.

The foot positions and servo angles are calculated 'in batches' for a given direction and turning angle using inverse kenimatics, and Xan's awesome Sin/Cos/Atan/ARCCos lookup table function (THANK YOU!). I can post the code if anyone is interested...

Video is on Youtube http://www.youtube.com/watch?v=S355-fRP8Kg&context=C4a0cfadADvjVQa1PpcFOD7XdYbEB1NV0GXMWsuUZqjd6-HqBQ-CI=



Last edited by Hybridsol on 2012-03-24 21:12, edited 1 time in total.

Rookie ( offline )
Posts: 11
Posted: 2012-03-14 18:09 
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 (8*6)=48
   y_foot var sword(48) ;x10 Y foot position in sequence (8*6)=48
   z_foot var sword(48) ;x10 Z foot position in sequence (8*6)=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)\20000*8/15*flip(i*3+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.0*20.0\2,"v",13]
HSEROUT ["VL voltage is: ",real (TOFLOAT hservostate 33)/1024.0*20.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(leg*8) = ABS(x_cen(leg)) ;Hip offset + thigh
      y_foot(leg*8) = 0
      GOSUB IK_foot[leg*8] ;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-12*ABS(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(ileg*8)<>height(ileg) OR height_delta(ileg)<>(z_foot(ileg*8+7)-z_foot(ileg*8)) 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(leg*3)*(HH_ang(leg*8+leg_seq(leg))))
      HVdist = ABS(HSERVOPOS(HV_pin(leg))-HV_offset(leg)-flip(leg*3+1)*(HV_ang(leg*8+leg_seq(leg))))
      Kdist = ABS(HSERVOPOS(K_pin(leg))-K_offset(leg)-flip(leg*3+2)*(K_ang(leg*8+leg_seq(leg))))
      ;
      HHspeed= 200*HHdist/step_time
      HVspeed= 200*HVdist/step_time ;200 = 10000/50
      Kspeed= 200*Kdist/step_time
      ;
      HSERVO [HH_pin(leg) \ flip(leg*3)*HH_ang(leg*8+leg_seq(leg))+HH_offset(leg)   \ HHspeed, |
            HV_pin(leg) \ flip(leg*3+1)*HV_ang(leg*8+leg_seq(leg))+HV_offset(leg) \ HVspeed, |
            K_pin(leg)  \ flip(leg*3+2)*K_ang(leg*8+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(leg*8+i)-HH_ang(Leg*8+i+1)) ;distance to next sequence position
         HVdist = ABS(HV_ang(Leg*8+i)-HV_ang(Leg*8+i+1))
         Kdist = ABS(K_ang(Leg*8+i)-K_ang(Leg*8+i+1))
         ;
         IF fastest_time < 200*HHdist/fast THEN
            fastest_time = 200*HHdist/fast
         ENDIF
         IF fastest_time < 200*HVdist/fast THEN
            fastest_time = 200*HVdist/fast
         ENDIF
         IF fastest_time < 200*Kdist/fast THEN
            fastest_time = 200*Kdist/fast
         ENDIF
         ;
      NEXT
      ;
      ;Add 7 to 0- lower foot
      HHdist = ABS(HH_ang(Leg*8+7)-HH_ang(Leg*8))
      HVdist = ABS(HV_ang(Leg*8+7)-HV_ang(Leg*8))
      Kdist = ABS(K_ang(Leg*8+7)-K_ang(Leg*8))
      ;
      IF fastest_time < 200*HHdist/fast THEN
         fastest_time = 200*HHdist/fast
      ENDIF
      IF fastest_time < 200*HVdist/fast THEN
         fastest_time = 200*HVdist/fast
      ENDIF
      IF fastest_time < 200*Kdist/fast THEN
         fastest_time = 200*Kdist/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 /10000*8/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(sleg*3)
   ;
   ;cos(K_angle) = (lenght^2 + height^2 - thigh^2 - shin^2) / 2 x (thigh + shin)
   ;
   IK_cos_K_angle = 10000*(IK_proj_rad*IK_proj_rad+z_foot(seq)*z_foot(seq)-Glength(sleg*3+1)*Glength(sleg*3+1) |
               -Glength(sleg*3+2)*Glength(sleg*3+2)) / (2*Glength(sleg*3+1)*Glength(sleg*3+2))
   IK_sin_K_angle = -SQR(100000000-IK_cos_K_angle*IK_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-AngleRad4*iconvert/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(sleg*3+1) + IK_cos_K_angle* Glength(sleg*3+2)/10000
   IK_temp_y = IK_sin_K_angle * Glength(sleg*3+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/10000*8/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(leg*8+i) = height(leg) ;foot down ->sequence 0-4,9-13,...
   NEXT
   ;
   FOR i = 5 to 7
      z_foot(leg*8+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(leg*3+1)+Glength(leg*3+2))*(GLength(leg*3+1)+Glength(leg*3+2)) - height(leg)*height(leg))+ GLength(leg*3)
   ;
   ;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 + 2*Len_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_max*Len_C + x_HH_min*Len_C)/Perimeter ; Len_B = Len_C
   y_cen(leg) = (y_HH_max*Len_C + y_HH_min*Len_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(leg*8+7) = x_foot(leg*8) ;AEP position
   y_foot(leg*8+7) = y_foot(leg*8)
   GOSUB IK_Foot[leg*8+7]
   ;
   ; Foot Up PEP   
   ;
   x_foot(leg*8+5) = x_foot(leg*8+4) ;PEP position
   y_foot(leg*8+5) = y_foot(leg*8+4)
   GOSUB IK_Foot[leg*8+5]
   ;
   ; Remaining posion is center
   ;
   x_foot(leg*8+6) = x_cen(leg)
   y_foot(leg*8+6) = y_cen(leg)
   GOSUB IK_Foot[leg*8+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(leg*8+i) = x_cen(leg) + ccos4 * min_radius * (2-i)/20000
         y_foot(leg*8+i) = y_cen(leg) + ssin4 * min_radius * (2-i)/20000
      ;
         GOSUB IK_Foot[leg*8+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) / 2*d -> since r2 = d -> a = r1^2 / 2*d
      ;
      A_turn = 100*min_radius*min_radius/(2*cen_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_radius*100 <= A_turn THEN
         ;Oh crap. Well, this sucks. Let's, um, pick another turn point that's like real close...
         WHILE min_radius*100 <= 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 = 100*min_radius*min_radius/(2*cen_dist)
         WEND
         ;
      ENDIF
      ;
      ;H = SQRT(r1^2 - a^2)
      H_turn = SQR(min_radius*min_radius*10000-A_turn*A_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_dist*100) *turnLRC/ABS(turnLRC)
      ry_turn =  (cen_x_turn-cen_x_in)*H_turn/(cen_dist*100) *turnLRC/ABS(turnLRC)
      ;
      x3_turn = cen_x_in + (cen_x_turn-cen_x_in)*A_turn/(cen_dist*100)
      y3_turn = cen_y_in + (cen_y_turn-cen_y_in)*A_turn/(cen_dist*100)
      ;AEP position
      x_foot(leg*8) = x3_turn + rx_turn-x_offset(leg)
      y_foot(leg*8) = y3_turn + ry_turn-y_offset(leg)
      ;PEP position
      x_foot(leg*8+4) = x3_turn - rx_turn-x_offset(leg)
      y_foot(leg*8+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_i*1800/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_i*1800/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(leg*8+2) = x_cen(leg)
      y_foot(leg*8+2) = y_cen(leg)
      ; Avg value for other points
      x_foot(leg*8+1) = (x_foot(leg*8) + x_cen(leg))/2
      y_foot(leg*8+1) = (y_foot(leg*8) + y_cen(leg))/2
      x_foot(leg*8+3) = (x_foot(leg*8+4) + x_cen(leg))/2
      y_foot(leg*8+3) = (y_foot(leg*8+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(leg*8+i), ", ", SDEC y_foot(leg*8+i)]            
         GOSUB IK_foot [leg*8+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/5*60 ->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+(31*3/4) to 128+(150*3/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 = 100*joy_speed/Joy_shighslope + Joy_shighinter 
   ELSEIF Joy_speed < Joy_speed_center - Joy_db/2
      Speed = 100*Joy_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 = 100*joy_turn/Joy_shighslope + Joy_shighinter 
   ELSEIF Joy_turn < Joy_turn_center - Joy_db
      turnLRC = 100*Joy_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_time*5 ;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 ((AtanX*AtanX*c4DEC) + (AtanY*AtanY*c4DEC)); CHANGED XYhyp2 to LONG variable
  GOSUB GetArcCos [AtanX*c6DEC / XYHyp2]
 
  Atan4 = AngleRad4 * (AtanY/ABS(AtanY)) ;Add sign
return Atan4
;-------------------------------------------------------------------------------------------------------


Last edited by Hybridsol on 2012-03-24 20:49, edited 4 times in total.

User avatar
Guru ( offline )
Posts: 9258
Posted: 2012-03-15 09:41 
Awesome! Thanks for sharing!

_________________
Jim Frye, the Robot Guy
http://www.lynxmotion.com
I've always tried to do my best...


User avatar
Guru ( offline )
Posts: 3298
Posted: 2012-03-15 14:12 
great little project. what i like about this is its very simple control set up. cant be any more simple and i love it.
one of the main features i like about this is the code. its great how the legs stop in motion were they are, rather than returning to a neutral position. much more life like.

_________________
Jonny Poole,
Freelance Concept Art & Animatronics, 3D Art and 3D Modelling, as well as Physical Modelling, Mechanical design, and illustration.
http://www.innerbreed.co.uk
Create, Innovate, Inspire.


Rookie ( offline )
Posts: 11
Posted: 2012-03-18 03:00 
Thanks for the feedback, it means a lot!

Right now I just cycle through the steps based on the joystick input witht the tripod gait, kinda crude, but it works...

I just finshed digesting a paper using 'CCN' gait generation (automatic gait generation, regardless of foot position) and it works pretty well in the spreadsheet. I'll post again when I get it working... hopefully the poto eye will be connected by then ;)

After that I'll try to add foot contact sensors, then the real magic can begin!

Here's a link to the paper:http://digital.csic.es/bitstream/10261/30549/1/doc1.pdf


Rookie ( offline )
Posts: 11
Posted: 2012-03-24 21:32 
New video of working code- obstable avoidance with one Sharp rangefinder eye

I sid "least squares" in the vid, it's actually sum of the squares, and linear vector field (two values), with moving window to find furthest point away. It uses that vector to steer the hex.

Completely autonomous, except when I pick it up of course... :lol:



User avatar
Guru ( offline )
Posts: 1851
Posted: 2012-03-24 22:50 
I like the fact that it change speed.
Look more "Insect" like.. ;)

Real good job..

_________________
Eric Nantel - DiaLFonZo
UAV/Drones Specialist

RobotShop inc.
Putting Robotics at your service!™
www.robotshop.com

Lynxmotion
Imagine it. Build it. Control it.™
http://www.Lynxmotion.com


User avatar
Guru ( offline )
Posts: 9258
Posted: 2012-03-26 10:10 
Hybridsol wrote:
New video of working code- obstable avoidance with one Sharp rangefinder eye


Very well done! :D

_________________
Jim Frye, the Robot Guy
http://www.lynxmotion.com
I've always tried to do my best...


Rookie ( offline )
Posts: 29
Posted: 2013-04-06 00:58 
Can u share the code cos I really need to understand how u did it. Im very new too I hope u share it with a lil description :)
thank u

_________________
BE JAN
------------------


New ( offline )
Posts: 2
Posted: 2013-04-23 12:44 
Hi there,

I am converting your code to run "hopefully" under the Arduino platform, I need to credit you as the original writer of the code. Can post the details you would like placed in the code.

Thanks,


User avatar
Guru ( offline )
Posts: 4869
Posted: 2013-04-23 13:24 
FYI - I don't think he has logged on to this forum in about a year.

Depending on what you wish to do, there are versions of the Phoenix code (Originally written by the member Xan) that has been ported to run on the Arduino environment. This includes the version that I with the help of others like Zenta ported and are used in several different configurations. You can find that work up on github\kurte\arduino_phoenix_parts

Kurt


 

All times are UTC - 5 hours [ DST ]. It is currently 2014-07-24 15:19
Feedback Form
Feedback Form