Inverse Kinematics for a robot. Question with math

QuantumRI

I have been working on this project for a while and hit a wall.
Below is my code and some pictures.· I·made the Hexapod and designed the controller board below.·
The·code is·working but I would like to add a ramping feature to allow smoother movement at different speeds.
I have tried to describe the code and give some values that are returned thru TV_text.
The math below solves·IK angles of two servos based·on geometric solutions. (2 Servos at the moment but more eventually)
The code is a work in progress and I plan on making it more advanced where I slowly and the hip servo.· Then make movements from the center of the body.
I have an issue with DS14 and DS15 equations towards the end of the code.· I have written comments into the code and where my problem is aswell.

''**********************************************
''*  QRIK V3.0                                 *
''*  Author: Paul Krasinski                    *
''*  Copyright (c) 2008 Quantum Robotics Inc.  *
''*  See end of file for terms of use.         *
''**********************************************
'' 
CON
        _clkmode        = xtal1 + pll16x
        _xinfreq        = 5_000_000
        Degrees         = (180.0 / pi)
        Radians         = (pi / 180.0)
        SD              = (180.0/1700.00)
        L1              = 8.0  '8mm Length Section 2
        L2              = 11.0 '11mm Length Section 1
               
OBJ
        SERVO  : "Servo32v3" 
       'term    : "tv_terminal"  Not Used to much space taken
        term    : "tv_text"
        f       : "Float32full"
        fp      : "FloatString"
Var     Long Time      
        Long Loop
 
 
PUB start | Temp
    f.start                                          'Start Float32Full Object
    term.start(24)                                   'Start TV _Terminal on pins 24,25,26
    
    repeat temp from 2 to 21                         'Preset ALL servo's to center position
      SERVO.Set(temp,1500)                              
    
    SERVO.Start                                       'Start Servo32v3 object
   Time := 80_000_000
   Repeat                                              
    Repeat Loop from 1 to 1000
     ' Leg1(8.0,-11.0)                               'Cordinate system for the leg.  Based on X Y with Servo 2 as the origin.  Below in a pic the front right is Servo 1
       Leg2(8.0,-12.0,6.0,-7.0)                      'This is the part I am trying to ramp from 8,-12 to 6,-7.
      Waitcnt (cnt + time)
    
    Repeat Loop From 1 to 1000
       Leg2(6.0,-7.0,8.0,-12.0)
       Waitcnt (cnt + time)
 
 
***I have just included the code for one of the legs since it's the same for all since.  There is only one different between l and r side
   this is the value is S14 & S15 is 2350.  This is to mirror the movements on the other side.
 
 
 
PRI Leg2(X,Y,XA,YA)|C, Q1, Q2, T1R, T2R, DS14, DS15, CA, Q1A, Q2A, T1RA, T2RA, S14A, S15A, DS14A, DS15A, S14B, S15B, S14, S15, Change14, Change15
 
  Case Loop
     1:
    
       C := f.fsqr(f.fadd((f.fmul(X,X)),(f.fmul(Y,Y)))) 'Pythagorean Theorem
       Q1 := f.fmul(degrees,F.atan2(Y,X)) '(y,x)
       Q2 := f.fmul(degrees,F.Acos((f.fdiv((f.fadd((f.fsub((f.fmul(L1,L1)),(f.fmul(L2,L2)))),(f.fmul(C,C)))),(f.fmul((f.fmul(2.0,L1)),C))))))
       T1R := f.fadd(Q1,Q2)    
       T2R := f.fmul(degrees,F.Acos((f.fdiv((f.fsub((f.fadd((f.fmul(L1,L1)),(f.fmul(L2,L2)))),(f.fmul(C,C)))),(f.fmul((f.fmul(L1,L2)),2.0))))))
       T1R := f.fabs((f.fsub(T1R,90.0)))
    
       CA := f.fsqr(f.fadd((f.fmul(XA,XA)),(f.fmul(YA,YA)))) 'Pythagorean Theorem
       Q1A := f.fmul(degrees,F.atan2(YA,XA)) '(y,x)
       Q2A := f.fmul(degrees,F.Acos((f.fdiv((f.fadd((f.fsub((f.fmul(L1,L1)),(f.fmul(L2,L2)))),(f.fmul(CA,CA)))),(f.fmul((f.fmul(2.0,L1)),CA))))))
       T1RA := f.fadd(Q1A,Q2A)    
       T2RA := f.fmul(degrees,F.Acos((f.fdiv((f.fsub((f.fadd((f.fmul(L1,L1)),(f.fmul(L2,L2)))),(f.fmul(CA,CA)))),(f.fmul((f.fmul(L1,L2)),2.0))))))
       T1RA := f.fabs((f.fsub(T1RA,90.0)))
      
       S14 := f.ftrunc((f.fadd((f.fdiv(T1R,SD)),650.0)))              This is the value used for servo movement which is plugged in below. These work and move the servos correctly
       S15 := f.ftrunc((f.fadd((f.fdiv(T2R,SD)),650.0)))
       S14A := f.ftrunc((f.fadd((f.fdiv(T1RA,SD)),650.0)))
       S15A := f.ftrunc((f.fadd((f.fdiv(T2RA,SD)),650.0)))
 
       DS14 := f.fadd((f.fdiv(T1R,SD)),650.0)                         This value actually shows a number that is understandable EX DS14= 1500 which is center of a servo
       DS15 := f.fadd((f.fdiv(T2R,SD)),650.0)                         These 4 lines are the same as above with out be trunc.  I dont understand why they need to be trunc
       DS14A := f.fadd((f.fdiv(T1RA,SD)),650.0)                       in order for 23v3 to work correctly.  Currently 
       DS15A := f.fadd((f.fdiv(T2RA,SD)),650.0)
       
       Change14 := f.fdiv((f.fsub(DS14,DS14A)),1000.0)            'I am taking the first X value and sub from XA then div by 1000 for resolution.  This gives me 1000 
       Change15 := f.fdiv((f.fsub(DS15,DS15A)),1000.0)             different x value steps.  So I have a nice ramping feature
       
       term.str(string("DS14 = "))                              'I use these few lines to see actual values on a screen. I change these back forth saves time from rewriting them
       term.str(fp.floattostring(DS14))
       term.out(13)
       term.str(string("DS15 = "))
       term.str(fp.floattostring(DS15))
       term.out(13)
       term.str(string("Change 14 = "))
       term.str(fp.floattostring(change14))
       term.out(13)
       term.str(string("Change 15 = "))
       term.str(fp.floattostring(Change15))
       term.out(13)
       
  DS14 := f.fadd(DS14,Change14)                                   'The difference/steps between the cordiates added to the orginal to the movement.  
  DS15 := f.fadd(DS15,Change15)                                    I need to trunc this in order for servo set to move otherwise the output is 0.
                                                                   I cant figure this part out.  I am taking the change14 adding to the start X value
                                                                   and I want to ramp it to the XA value by increasing this every time the loop increases from 
                                                                   1 to 1000.  This works and I have seen it increment on tv out but SERVO set wants a trunc value.
                                                                   When I trunc this it turns to 0 since the value is a decimal.  
                                                                ***I think my probably is to use another method that works with decimals but I have tried some many different
                                                                   things that at this point I am totaly lost.

  Servo.set(14,S14)                                            'These two lines control the pulse to the servo
  Servo.Set(15,S15)               

  term.str(string("S14 = "))                                    'Next few lines are just to visually see thre results
  term.str(fp.floattostring(DS14))
  term.out(13)
  term.str(string("S15 = "))
  term.str(fp.floattostring(DS15))
  term.out(13)
  Waitcnt (cnt + time)
 
{{
┌──────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────┐
│                                                   TERMS OF USE: MIT License                                                  │                                                            
├──────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────┤
│Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation    │ 
│files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy,    │
│modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software│
│is furnished to do so, subject to the following conditions:                                                                   │
│                                                                                                                              │
│The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.│
│                                                                                                                              │
│THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE          │
│WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR         │
│COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,   │
│ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.                         │
└──────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────┘
}}

If something is not that clear I will eloborate.·

Post Edited (QuantumRI) : 11/7/2008 6:02:31 AM GMT

Ramboman

Very nice project.

I am planning something similar with only 4 legs (12 servos).

My intention is to be able to climb a stair.

I can't answer your question, but I have another:

"Why do you work in floating point?"

You have already a SINE table in Eprom and·the ASM access metthod is fully described.

This would be much much faster to compute in integer.

From a doctorate thesis of the university of Brussels, I retain the following:

Level A: Global control; in my case an RC link.

Level B: Global attitude; give the "global" instructions to the legs to follow the instructions.

Level C: Leg control; for each leg, compute and control each servo, probably one cog per leg...

Good Luck

Roland