{{ GaitNomad.spin }} ' ============================================================================== ' ' File...... GaitNomad.spin ' Purpose... Gait object for the CrustCrawler Nomad ' Author.... (C) 2006 Steven R. Norris -- All Rights Reserved ' E-mail.... norris56@comcast.net ' Started... 11/27/2006 ' Updated... 11/28/2006 ' ' ============================================================================== ' Note: Switch editor to Documentation mode to read all the text. ' ------------------------------------------------------------------------------ ' Program Description ' ------------------------------------------------------------------------------ {{ Author: Steve Norris Email : norris56@comcast.net This object is a reusable walking engine for the Nomad from CrustCrawler. You can use it for all your Propeller-based Nomad applications. This object uses the same configuration as the Nomad GaitPIC. Consult the wiring diagram for the Nomad GaitPIC for details. This object requires two Cogs. One is for the walking engine and the other is for the serial communications to the Parallax Servo Controller. The serial communications to the PSC runs at 38400 baud. The Start, Stop and IsActive methods are used for Cog maintenance. The SetGait, SetGaitWait, IsWalking, Home, and Halt methods are used to control the walking engine. The Parallax Servo Controller (PSC) is connected to the pin specified in the call to the Start method. The bulk of the walking engine code can be found in the Main, Walk and WalkTable methods. These methods use the gait tables found in the DAT section. Note: Set the constant DigiVert to FALSE if you are not using digital robot servos for the vertical lift. All values in the gait tables assume you are using HS-645 servos for the horizontal and foot. Adjust them as needed for your own configuration. To use this object simply include it in the OBJ section of your application code. Be sure to call the Start method first before you call any of the other methods. }} ' ------------------------------------------------------------------------------ ' Revision History ' ------------------------------------------------------------------------------ {{ 0648a - This is the first version }} CON ' ------------------------------------------------------------------------------ ' Constants ' ------------------------------------------------------------------------------ _clkmode = xtal1 + pll16x _xinfreq = 5_000_000 ' Leg positions CenterPos = 750 ' Horizontal RaisePos = 625 ' Vertical - normal RaiseHiPos = 850 ' Vertical - high LowerPos = 300 ' Vertical RaisePosDv = 700 ' Digital Vertical - normal RaiseHiPosDv = 850 ' Digital Vertical - high LowerPosDv = 600 ' Digital Vertical ' Gait flags (encoded in horizontal position) GaitFlags = $7FF ' Gait flags mask Raise = $800 ' Raise flag Lower = $000 ' Lower flag ' Set to TRUE if using digital servos DigiVert = TRUE DAT ' Walk D1 long CenterPos-85 | Lower, CenterPos+55 ' Setup A long CenterPos+220 | Lower, CenterPos-65 long CenterPos-95 | Lower, CenterPos-10 long CenterPos-220 | Raise, CenterPos-45 ' Reset B long CenterPos+75 | Raise, CenterPos+85 long CenterPos+95 | Raise, CenterPos-30 long CenterPos-180 | Lower, CenterPos-65 ' Power A long CenterPos+105 | Lower, CenterPos+40 long CenterPos+35 | Lower, CenterPos-20 long CenterPos-220 | Lower, CenterPos-45 ' Setup B long CenterPos+75 | Lower, CenterPos+85 long CenterPos+95 | Lower, CenterPos-30 long CenterPos-85 | Raise, CenterPos+55 ' Reset A long CenterPos+220 | Raise, CenterPos-65 long CenterPos-95 | Raise, CenterPos-10 long CenterPos-95 | Lower, CenterPos+55 ' Power B long CenterPos+180 | Lower, CenterPos-30 long CenterPos-35 | Lower, CenterPos-50 long $FF ' Spin CW D2 long CenterPos-90 | Lower, CenterPos ' Setup A long CenterPos-90 | Lower, CenterPos long CenterPos-90 | Lower, CenterPos long CenterPos-90 | Raise, CenterPos ' Reset B long CenterPos-90 | Raise, CenterPos long CenterPos-90 | Raise, CenterPos long CenterPos | Lower, CenterPos ' Power A long CenterPos | Lower, CenterPos long CenterPos | Lower, CenterPos long CenterPos-90 | Lower, CenterPos ' Setup B long CenterPos-90 | Lower, CenterPos long CenterPos-90 | Lower, CenterPos long CenterPos-90 | Raise, CenterPos ' Reset A long CenterPos-90 | Raise, CenterPos long CenterPos-90 | Raise, CenterPos long CenterPos | Lower, CenterPos ' Power B long CenterPos | Lower, CenterPos long CenterPos | Lower, CenterPos long $FF ' Spin CCW D3 long CenterPos+70 | Lower, CenterPos ' Setup A long CenterPos+70 | Lower, CenterPos long CenterPos+70 | Lower, CenterPos long CenterPos+70 | Raise, CenterPos ' Reset B long CenterPos+70 | Raise, CenterPos long CenterPos+70 | Raise, CenterPos long CenterPos | Lower, CenterPos ' Power A long CenterPos | Lower, CenterPos long CenterPos | Lower, CenterPos long CenterPos+70 | Lower, CenterPos ' Setup B long CenterPos+70 | Lower, CenterPos long CenterPos+70 | Lower, CenterPos long CenterPos+70 | Raise, CenterPos ' Reset A long CenterPos+70 | Raise, CenterPos long CenterPos+70 | Raise, CenterPos long CenterPos | Lower, CenterPos ' Power B long CenterPos | Lower, CenterPos long CenterPos | Lower, CenterPos long $FF ' Servo sequence order for each direction LegSeqs byte 0,8,20,16,24,4 ' Direction 0 - 0 degrees (forward) byte 4,16,24,20,0,8 ' Direction 1 - 60 degrees byte 8,20,0,24,4,16 ' Direction 2 - 120 degrees byte 16,24,4,0,8,20 ' Direction 3 - 180 degrees (reverse) byte 20,0,8,4,16,24 ' Direction 4 - 240 degrees byte 24,4,16,8,20,0 ' Direction 5 - 300 degrees VAR ' ------------------------------------------------------------------------------ ' Variables ' ------------------------------------------------------------------------------ ' Cog maintenance variables byte m_Cog long m_Stack[50] ' Parallax Serial Controller (PSC) Pin byte m_PscTx ' Requested gait byte m_GaitCode byte m_GaitDir byte m_GaitSpeed long m_GaitCounter byte m_LastGait ' Current leg sequence byte m_LegSeq[6] OBJ PscComm : "FullDuplexSerial" PUB Start(PinPsc) : Success ' ------------------------------------------------------------------------------ ' Start this resource (cog) ' ------------------------------------------------------------------------------ {{ Starts the walking engine in a seperate cog. Pass the pin number that is connected to the Parallax Servo Controller (PSC). Returns a true if the startup was successful. }} m_PscTx := PinPsc m_LastGait := $FF Stop Success := (m_Cog := cognew(Main,@m_Stack) + 1) PUB Stop ' ------------------------------------------------------------------------------ ' Stop this resource (cog) ' ------------------------------------------------------------------------------ {{ Stops the execution of the walking engine. }} if m_Cog cogstop(m_Cog~ - 1) PUB IsActive :YesNo ' ------------------------------------------------------------------------------ ' Is this resource (cog) running? ' ------------------------------------------------------------------------------ {{ Determines if the walking engine is running in a cog. }} YesNo := m_Cog > 0 PUB SetGait(Gait, Direction, Speed, GaitCount) ' ------------------------------------------------------------------------------ ' Start walking ' ------------------------------------------------------------------------------ {{ Sets the current gait. This method is non-blocking. Pass the Gait code, direction, speed and the number of steps to take. Passing -1 for GaitCount results in continous walking. Gaits: 0 - Walk 1 - Spin CCW 2 - Spin CW Directions: 0 ....... 0 degrees (forward) 1 ....... 60 degrees 2 ....... 120 degrees 3 ....... 180 degrees (reverse) 4 ....... 240 degrees 5 ....... 300 degrees Speeds: 0 - Fast 1 - Normal 2 - Slow 3 - Slow with high step }} m_GaitCode := Gait m_GaitDir := Direction m_GaitSpeed := Speed m_GaitCounter := GaitCount PUB SetGaitWait(Gait, Direction, Speed, GaitCount) ' ------------------------------------------------------------------------------ ' Start walking and wait to complete ' ------------------------------------------------------------------------------ {{ Same as SetGait but this method will wait for the command to complete. Will not wait if you pass -1 for continous walking. }} m_GaitCode := Gait m_GaitDir := Direction m_GaitSpeed := Speed m_GaitCounter := GaitCount repeat while m_GaitCounter > 0 PUB Home(type) ' ------------------------------------------------------------------------------ ' Home the legs ' ------------------------------------------------------------------------------ {{ Home all the legs. There are 3 homing sequences: 1 - lower all legs and then center them 2 - raise, center and lower each leg one at a time 3 - just lower all legs }} Halt Pause_ms(1000) case type 1: Home1 2: Home2 3: Home3 other: return PUB Halt ' ------------------------------------------------------------------------------ ' Stop walking ' ------------------------------------------------------------------------------ {{ Stops the current gait. }} m_GaitCode := 0 m_GaitCounter := 0 PUB IsWalking : YesNo ' ------------------------------------------------------------------------------ ' Are we still walking? ' ------------------------------------------------------------------------------ {{ Determines if we are currently executing a gait. }} YesNo := m_GaitCounter > 0 or m_GaitCounter == -1 PRI Main ' ------------------------------------------------------------------------------ ' Main for the Walking Engine ' ------------------------------------------------------------------------------ ' Initialize PSC serial communications ' Step it up to 38400 baud PscComm.Start(-1, m_PscTx, 0, 2400) SetPscBaudRate(1) ' Now restart the cog using the higher baud PscComm.Stop PscComm.Start(-1, m_PscTx, 0, 38400) ' Main loop ' Watch for a gait count greater than zero repeat if m_GaitCounter > 0 or m_GaitCounter == -1 Walk if(m_GaitCounter <> 0 and m_GaitCounter <> -1) m_GaitCounter-- PRI Walk | i, ptr, VertRamp, HorzRamp, FootRamp, HiStep ' Setup ramp speeds HiStep := FALSE case m_GaitSpeed 0: VertRamp := 6 HorzRamp := 9 FootRamp := 9 1: VertRamp := 7 HorzRamp := 11 FootRamp := 11 2: VertRamp := 7 HorzRamp := 12 FootRamp := 12 3: VertRamp := 7 HorzRamp := 14 FootRamp := 14 HiStep := TRUE other: return ' Setup servo sequence for this direction ptr := m_GaitDir * 6 repeat i from 0 to 5 m_LegSeq[i] := LegSeqs[ptr + i] ' Select the gait table case m_GaitCode 0: ' Walk ptr := @D1 1: ' Spin CW ptr := @D2 2: ' Spin CCW ptr := @D3 other: return ' If walking with a new gait, transition from the last one if m_GaitCode <> m_LastGait WalkTable(TRUE, ptr, VertRamp, HorzRamp, FootRamp, HiStep) ' Now walk using selected gait table WalkTable(FALSE, ptr, VertRamp, HorzRamp, FootRamp, HiStep) m_LastGait := m_GaitCode PRI WalkTable(Transition, GaitTable, VertRamp, HorzRamp, FootRamp, HiStep) | IdxPos, IdxSeq, Servo, HorzPos, FootPos IdxPos := 0 IdxSeq := 0 if m_GaitCounter == 0 return if Transition Home3 IdxPos := 24 HorzPos := LONG[GaitTable][IdxPos] repeat while HorzPos <> $FF if m_GaitCounter == 0 return Servo := m_LegSeq[IdxSeq] ' Vertical servo Send2PSC(Servo + 1,VertRamp,GetVertPos(HiStep, HorzPos & Raise)) ' Horizontal servo HorzPos &= GaitFlags ' Mask off gait flags to get the horizontal position Send2PSC(Servo,HorzRamp,HorzPos) ' Foot servo FootPos := LONG[GaitTable][IdxPos + 1] Send2PSC(Servo + 2,FootRamp,FootPos) ' Get next servo position value IdxPos += 2 if(Transition and IdxPos == 30) HorzPos := $FF else HorzPos := LONG[GaitTable][IdxPos] ' Get next servo address in the sequence IdxSeq += 1 if(IdxSeq == 3 or IdxSeq == 6) ' Let group (A or B) move to their new positions WaitServo if IdxSeq == 6 IdxSeq := 0 PRI GetVertPos(HiStep,RaiseUp) if DigiVert if(RaiseUp) if(HiStep) return RaiseHiPosDv else return RaisePosDv else return LowerPosDv else if(RaiseUp) if(HiStep) return RaiseHiPos else return RaisePos else return LowerPos PRI Home1 | servo repeat servo from 0 to 8 step 4 HomeLeg1(servo) repeat servo from 16 to 24 step 4 HomeLeg1(servo) Pause_ms(1100) m_LastGait := $FF PRI HomeLeg1(servo) Send2PSC(servo,8,750) Send2PSC(servo + 1,8,GetVertPos(FALSE, FALSE)) Send2PSC(servo + 2,8,750) PRI Home2 | servo ' Lower all legs repeat servo from 1 to 9 step 4 Send2PSC(servo,8,GetVertPos(FALSE, FALSE)) repeat servo from 17 to 25 step 4 Send2PSC(servo,8,GetVertPos(FALSE, FALSE)) Pause_ms(500) ' Now center each leg repeat servo from 0 to 8 step 4 HomeLeg2(servo) repeat servo from 16 to 24 step 4 HomeLeg2(servo) m_LastGait := $FF PRI HomeLeg2(servo) Send2PSC(servo + 1,8,GetVertPos(FALSE, TRUE)) Pause_ms(100) Send2PSC(servo,8,CenterPos) Send2PSC(servo + 2,8,CenterPos) Pause_ms(200) Send2PSC(servo + 1,8,GetVertPos(FALSE, FALSE)) Pause_ms(200) PRI Home3 AllLower m_LastGait := $FF PRI AllLower | servo ' Lower all legs repeat servo from 1 to 9 step 4 Send2PSC(servo,8,GetVertPos(FALSE, FALSE)) repeat servo from 17 to 25 step 4 Send2PSC(servo,8,GetVertPos(FALSE, FALSE)) Pause_ms(500) PRI WaitServo case m_GaitSpeed 0: Pause_ms(240) 1: Pause_ms(255) 2: Pause_ms(350) 3: Pause_ms(475) PRI Send2PSC(Servo, Ramp, Position) ' Send a servo postion command to the servo controller PscComm.str(string("!SC")) PscComm.tx(Servo) PscComm.tx(Ramp) PscComm.tx(Position.byte[0]) PscComm.tx(Position.byte[1]) PscComm.tx($0D) Pause_ms(10) PRI SetPscBaudRate(Baud) ' Select the baud rate for the PSC ' 0 = 2400 ' 1 = 38400 PscComm.str(string("!SCSBR")) PscComm.tx(Baud) PscComm.tx($0D) Pause_ms(50) PRI Pause_ms(msDelay) waitcnt(cnt + ((clkfreq / 1000) * msDelay))