''********************************************** ''* Master Controller v3 * ''* Author: Bob Sweeney * ''* Start with clean slate - no radio control * ''********************************************** '' CON 'System Clock _xinfreq = 5_000_000 _clkmode = xtal1 + pll16x CR = 13 CLS = 16 Degrees = (180.0/pi) Radians = (pi / 180.0) '[BODY DIMENSIONS] All units are in 1/10" CoxaLength = 2.6 'Length of the Coxa [in] FemurLength = 6.0 'Length of the Femur [in] TibiaLength = 24.6 'Lenght of the Tibia [in] OBJ uarts : "pcFullDuplexSerial4FC" '1 COG for 4 serial ports F : "F32_1_6.spin" FS : "FloatString" Var long coxaAngle, tibiaAngle, femurAngle long L1, L, C, IKb, IKb1, IKb2 PUB StartProgram F.start uarts.Init 'uarts.AddPort(vm_port,vm_rx,vm_tx,vm_cts,vm_rts,UARTS#DEFAULTTHRESHOLD,UARTS#NOMODE,UARTS#BAUD9600) uarts.AddPort(1,31,30,UARTS#PINNOTUSED,UARTS#PINNOTUSED,UARTS#DEFAULTTHRESHOLD,UARTS#NOMODE,UARTS#BAUD115200) uarts.Start 'Start the ports pauseMSec(500) uarts.tx(1, CLS) Main Pub Main LegIK(1.0,24.0,10.0) PUB LegIK (IKX, IKY, IKZ) '[LEG INVERSE KINEMATICS] Calculates the angles of the tibia and femur for the given position of the feet L1 := f.fsqr(f.fadd(f.fmul(IKz,IKz),f.fmul(IKx,IKx))) L := f.fsub(L1, coxaLength) C := f.fsqr(f.fadd(f.fmul(IKy,IKy),f.fmul(L,L))) IKb := f.acos(f.fdiv((f.fsub((f.fadd((f.fmul(FemurLength,FemurLength)),(f.fmul(C,C)))),(f.fmul(TibiaLength,TibiaLength)))),(f.fmul((f.fmul(2.0,Femurlength)),C)))) IKb2 := f.atan2(L,IKy) IKb1 := f.acos(f.fdiv((f.fsub((f.fadd((f.fmul(tibiaLength,tibiaLength)),(f.fmul(C,C)))),(f.fmul(femurLength,femurLength)))),(f.fmul((f.fmul(2.0,tibialength)),C)))) femurAngle := f.fmul(f.fadd(IKb,IKb2), Degrees) tibiaAngle := f.fmul(f.fsub(IKb2,IKb1), Degrees) coxaAngle := f.fmul(f.atan2(IKz,IKx), Degrees) uarts.Str(1, String(13, "L1: ")) uarts.Str(1, fs.FloatToString(L1)) uarts.Str(1, String(13, "coxaLength: ")) uarts.Str(1, fs.FloatToString(coxaLength)) uarts.Str(1, String(13, "L: ")) uarts.Dec(1, fs.FloatToString(L)) uarts.Str(1, String(13, "C: ")) uarts.Dec(1, fs.FloatToString(C)) uarts.Str(1, String(13, "IKb: ")) uarts.Dec(1, fs.FloatToString(IKb)) uarts.Str(1, String(13, "IKb1: ")) uarts.Dec(1, fs.FloatToString(IKb1)) uarts.Str(1, String(13, "B2: ")) uarts.Dec(1, fs.FloatToString(IKb2)) uarts.Str(1, String(13, "femurAngle: ")) uarts.Dec(1, fs.FloatToString(femurAngle)) uarts.Str(1, String(13, "TibiaAngle: ")) uarts.Dec(1, fs.FloatToString(tibiaAngle)) uarts.Str(1, String(13, "CoxaAngle: ")) uarts.Dec(1, fs.FloatToString(coxaAngle)) uarts.Str(1, String(13)) '-------------------------------------------------------------------- PUB pauseMSec(Duration) '' Pause execution in milliseconds. '' Duration = number of milliseconds to delay waitcnt(((clkfreq / 1_000 * Duration - 3932) #> 381) + cnt) { ┌──────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────┐ │ 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. │ └──────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────┘ }