This is a continuation of the converstaion about making the serial communications between a PC and a hex running code from powerpod more robust.
This thread was started on the sticky thread talking about the powerpod serial communications command set.
I hacked up the output of powerpod to allow the PC to signal a new command by first sending a hex byte of FF. The pc program should very briefly pause and then send out the serial command. This program has not been tested yet, so try at your own risk. I wrote the update in such a way that if a new command has not been received it may try to repeat the last command. I only allow the simulated button presses to repeat once, but for the movement values I try to allow it to repeat up to 255 times before the PC gets a new command sent to the hex.
I hope it works
Kurt
P.S. - I only added a few variables, added the timer init code which also initialized a some variables, changed the beginning of the main loop and added the timer interrupt code at the end.
[code];
; Use Basic Micro ATOM PRO IDE 08.0.1.7
; *** not suitable for IDE 02.2.1.1 (there’s a specific version for this IDE)
; *** not suitable for IDE 05.3.0.0 (there’s a specific version for this IDE)
; Use Basic Atom PRO 28
; - Modified by forum member: KurtE
; - Modified version to use IRQ0 on S_IN/S_OUT for more handshake with PC
; - PC needs to first send a hex FF byte to signal BAP that a new command is coming.
; then it needs to briefly pause and then output the new command.
; In the future, maybe we should then add a acknowledge signal back from BAP.
;************************************************
;****** Basic Atom with SSC-32, PC control ******
;----------------- 3.42 3DOF-C ------------------
;---- Serial Port Control (Bot Board / PC) ------
;-------------- Round Body (H3-R) ---------------
;********** Bot Board Buzzer support ************
;***** Little Gripper / Pan & Tilt support ******
;**************** Deck support ******************
;************************************************
;** Programmer: Laurent Gay, [email protected] **
;************************************************
;
; let’s say that the 2 switches holes of the H3/H3R is the ‘rear’ of the robot
;
; *** using 3 ‘right’ legs and 3 ‘left’ legs ***
; put the SSC-32 card with the PC serial port looking to ‘Front’ (opposite to H3R switches)
;
; 3 right legs connections :
; Rear Right leg : Hip Horizontal : pin 0, Hip Vertical : pin 1, Knee : pin 2
; Middle Right leg : Hip Horizontal : pin 4, Hip Vertical : pin 5, Knee : pin 6
; Front Right leg : Hip Horizontal : pin 8, Hip Vertical : pin 9, Knee : pin 10
;
; 3 left legs connections :
; Rear Left leg : Hip Horizontal : pin 16, Hip Vertical : pin 17, Knee : pin 18
; Middle Left leg : Hip Horizontal : pin 20, Hip Vertical : pin 21, Knee : pin 22
; Front Left leg : Hip Horizontal : pin 24, Hip Vertical : pin 25, Knee : pin 26
;
; Default mode :
; Move the robot (2 axis translation) with the right joystick
; Turn with the left joystick X axis
; move Up/Down body with the left joystick Y axis
; lock/unlock the height with the left joystick push button (L3)
;
; Body move mode :
; Push ‘select’ button to activate this mode (push ‘select’ again to return to default mode)
; move the body horizontally (2 axis translation) with the right joystick
; the left joystick push button (L3) has a new function now, it selects between 4 control modes for the left joystick:
; 1)- (by default) move the body vertically + rotate around the vertical axis
; 2)- (push L3 to activate) move the body vertically + roll
; 3)- (push L3 again to activate) pitch + rotate around the vertical axis
; 4)- (push L3 again to activate) pitch + roll the body
; pushing L3 again will return to mode 1 (cycling)
;
; go to standby position with the ‘Triangle’ button, press again to go to walk position
; Attack posture with the ‘Cross’ button (*** only possible if legs are not moving )
; All = 1500 posture with the ‘Circle’ button ( only possible if legs are not moving )
; Learning to fly posture with the ‘Square’ button (uh ? nutty ?..it CAN fly…) ( only possible if legs are not moving )
; Change the legs ‘up’ position with the digital Up/Down buttons
;
; Change the gait speed with digital right/left buttons
;
; Presets buttons (only with ‘Default’ mode):
; - R1 => Tall grass
; - L1 => Tile floor
; - R2 => Body low
; - L2 => Default preset
;
; Little Gripper support on Pin 29 (Left/Right) and 30 (Open/Close) (only in ‘Body move’ mode)
; - R1 => Open
; - L1 => Close
; - R2 => Rotate Right
; - L2 => Rotate Left
;
; Deck support on Pin 31 (panning servo)
; allow to handle a deck (for camera, sensors, lights)
; the panning servo will follow the direction it walk
; Additional tilt servo support for 180° Deck on pin 28 (Round body only)
; When using a 180° deck + 2 GP2D12 back to back on a tilt servo
; the tilt servo will incline the ‘usable’ GP2D12 (looking the walking direction) to floor
; when walking at low speed, the angle increase (looking near)
; when walking at full speed, the angle is close to horizontal (looking far)
; when going opposite direction, the angle is mirrored to vertical,
; making the opposite GP2D12 looking near or far
;
; Start button toggles between the ‘knee angle shift’ and ‘normal’ mode
;
; R3 button : Horn
;
;*********************************************
;
;
;--------------------------------------------------------------------
;-------------Constants
;
DeadZone con 8
;*** SSC-32 card communication on pin 15 ***
SSC32 con p15
;Deck
Deck_PulseMin con 600
Deck_PulseMax con 2400
;Little Gripper
LGripLR_PulseMin con 750
LGripLR_PulseMax con 2250
LGripOC_PulseMin con 750
LGripOC_PulseMax con 2250
;Legs
HipH_AngleMin con 21 ;30°
HipH_AngleMax con 107 ;150°
HipH_PulseMin con 910
HipH_PulseMax con 2090
HipV_AngleMin con 25 ;35°
HipV_AngleMax con 103 ;145°
HipV_PulseMin con 960
HipV_PulseMax con 2040
Knee_AngleMin con 36 ;50°
Knee_AngleMax con 107 ;150°
Knee_PulseMin con 1107
Knee_PulseMax con 2090
;****************************************************
; 3DOF-A Leg Dimensions (TibiaAngle constant = 0)
;HipV_HipH con 38 ;1.50" = 38mm (1.50 * 25.4)
;Femur_Length con 57 ;2.25" = 57mm (2.25 * 25.4)
;Tibia_Length con 124 ;4.875" = 124mm (4.875 * 25.4)
; 3DOF-B Leg Dimensions (TibiaAngle constant = 0)
;HipV_HipH con 29 ;1.14" = 29mm (1.14 * 25.4)
;Femur_Length con 57 ;2.25" = 57mm (2.25 * 25.4)
;Tibia_Length con 108 ;4.25" = 108mm (4.25 * 25.4)
; 3DOF-C Leg Dimensions (TibiaAngle constant = 20)
;HipV_HipH con 29 ;1.14" = 29mm (1.14 * 25.4)
;Femur_Length con 57 ;2.25" = 57mm (2.25 * 25.4)
;Tibia_Length con 141 ;5.55" = 141mm (5.55 * 25.4)
; 3DOF-(Old) Leg Dimensions (TibiaAngle constant = 0)
;HipV_HipH con 32 ;1.25" = 32mm (1.25 * 25.4)
;Femur_Length con 70 ;2.75" = 70mm (2.75 * 25.4)
;Tibia_Length con 108 ;4.25" = 108mm (4.25 * 25.4)
;****************************************************
; 3DOF-C Leg Dimensions (TibiaAngle constant = 20)
HipV_HipH con 29 ;1.14" = 29mm (1.14 * 25.4)
Femur_Length con 57 ;2.25" = 57mm (2.25 * 25.4)
Tibia_Length con 141 ;5.55" = 141mm (5.55 * 25.4)
LegUpShiftMin con 30
LegUpShiftMax con 70
KneeShiftPulse con 200 ;range is 0 to 255 (activated by pushing start button, affect the legs when up)
TibiaAngle con 0 ;range is -20 (interior) to 20 (exterior), 0 is vertical, this is ‘all time’ tibia angle
;ACos
ArcCos Bytetable 64,64,63,63,63,62,62,62,61,61,61,60,60,60,59,59,|
59,59,58,58,58,57,57,57,56,56,56,55,55,55,54,54,|
54,53,53,53,52,52,52,51,51,51,50,50,50,49,49,49,|
48,48,48,47,47,46,46,46,45,45,45,44,44,44,43,43,|
42,42,42,41,41,41,40,40,39,39,39,38,38,37,37,37,|
36,36,35,35,35,34,34,33,33,32,32,31,31,31,30,30,|
29,29,28,28,27,27,26,25,25,24,24,23,23,22,21,21,|
20,19,19,18,17,16,15,15,14,13,11,10,09,07,05,00
; Don’t use ByteTable instead, it takes too much memory
RRHH con “0” ;Rear Right leg : Hip Horizontal : pin 0
RRHH2 con “0”
RRHV con “0” ;Rear Right leg : Hip Vertical : pin 1
RRHV2 con “1”
RRK con “0” ;Rear Right leg : Knee : pin 2
RRK2 con “2”
MRHH con “0” ;Middle Right leg : Hip Horizontal : pin 4
MRHH2 con “4”
MRHV con “0” ;Middle Right leg : Hip Vertical : pin 5
MRHV2 con “5”
MRK con “0” ;Middle Right leg : Knee : pin 6
MRK2 con “6”
FRHH con “0” ;Front Right leg : Hip Horizontal : pin 8
FRHH2 con “8”
FRHV con “0” ;Front Right leg : Hip Vertical : pin 9
FRHV2 con “9”
FRK con “1” ;Front Right leg : Knee : pin 10
FRK2 con “0”
RLHH con “1” ;Rear Left leg : Hip Horizontal : pin 16
RLHH2 con “6”
RLHV con “1” ;Rear Left leg : Hip Vertical : pin 17
RLHV2 con “7”
RLK con “1” ;Rear Left leg : Knee : pin 18
RLK2 con “8”
MLHH con “2” ;Middle Left leg : Hip Horizontal : pin 20
MLHH2 con “0”
MLHV con “2” ;Middle Left leg : Hip Vertical : pin 21
MLHV2 con “1”
MLK con “2” ;Middle Left leg : Knee : pin 22
MLK2 con “2”
FLHH con “2” ;Front Left leg : Hip Horizontal : pin 24
FLHH2 con “4”
FLHV con “2” ;Front Left leg : Hip Vertical : pin 25
FLHV2 con “5”
FLK con “2” ;Front Left leg : Knee : pin 26
FLK2 con “6”
;Little Gripper Pin
LGripLR Con “2” ;Little Gripper Left/Right : pin 29
LGripLR2 Con “9”
LGripOC Con “3” ;Little Gripper Open/close : pin 30
LGripOC2 Con “0”
; Deck Pin
DeckP Con “3” ;Deck Panning servo : pin 31
DeckP2 Con “1”
; Additional Tilt servo for 180° Deck
DeckTilt Con “2”
DeckTilt2 Con “8”
;--------------------------------------------------------------------
;-------------Variables
Index var Byte
DualShock var Byte(7)
LastButton var Byte(2)
Steps var Byte
FlipFlap var Bit
FlagOff var Bit
MovesDelay var Sbyte
HeightAdjust var Bit
HeightShift var Sbyte(6)
KneeShift var Byte(7)
Steering var Sbyte
Height var Sbyte
LastHeight var Sbyte
HeightLimit var Byte
XSpeed var Sbyte
YSpeed var Sbyte
Roll var Sbyte
Pitch var Sbyte
Rotate var Sbyte(6)
DCoord var Byte
DAngle var Byte
NbSteps var Byte
GaitSpeed var Byte
GaitSpeedTmp var Byte
StepFlag var Byte
TmpXPos var Long
TmpYPos var Long
TmpZPos var Long
TmpDistance var Long
TmpSEW var Word
TmpSEWSEW var Long
TmpCos var Long
TmpAngle var Sbyte
XPos var Sword(6)
YPos var Sword(6)
ZPos var Sword(6)
XPos2 var Sword(6)
YPos2 var Sword(6)
ZPos2 var Sword(6)
Distance var Sword
HipH_Angle var Sbyte
HipV_Angle var Sword
Knee_Angle var Sword
HipH_Pulse var Word(6)
HipV_Pulse var Word(6)
Knee_Pulse var Word(6)
LittleGripOCPulse var Word
LittleGripLRPulse var Word
Deck_Pulse var Word
DeckTilt_Pulse var Word
LegUpShift var Byte
GaitKind var Bit
Freeze var Bit
Tripod var Bit
LockLegs var Bit
LeftStickMode var Byte
CheckSum var Byte
; new for modified version
abNewCommand var byte(7)
fNewCommand var bit
nRepeatCommand var byte ; we set up a maximum time to repeat a command to make sure we don’t get a runaway
;--------------------------------------------------------------------
;***************
;*** Program ***
;***************
;-------------Init
clear
LastButton(0) = 255
LastButton(1) = 255
LittleGripOCPulse = 1500
LittleGripLRPulse = 1500
HeightLimit = 25 - ABS(TibiaAngle / 2)
; initialize PC communications, use an Int0
ONINTERRUPT IRQ0INT, HANDLE_S_IN_INT
;==============================================================================
; Initialize communications with PC
; lets setup to get interrupted if we get a rising edge on IRQ0.
;==============================================================================
fNewCommand = 0 ; make sure we don’t think we have a new command yet…
nRepeatCommand = 0; ; nothing to repeat
PMR1.bit4 = 1 ; enable pin to IRQ0 interrupt instead of normal I/O
IEGR1.bit0 = 0 ; Interrupt IRQ0 on falling edge
enable irq0
ienr1.bit0 = 1
enable ; global enable interrupts to be processed
pause 500
;-------------Init SSC-32 with pulse offsets
serout SSC32,i38400,"#", |
RRHH,RRHH2,“po0 #”,RRHV,RRHV2,“po0 #”,RRK,RRK2,“po0 #”, |
MRHH,MRHH2,“po0 #”,MRHV,MRHV2,“po0 #”,MRK,MRK2,“po0 #”, |
FRHH,FRHH2,“po0 #”,FRHV,FRHV2,“po0 #”,FRK,FRK2,“po0 #”, |
RLHH,RLHH2,“po0 #”,RLHV,RLHV2,“po0 #”,RLK,RLK2,“po0 #”, |
MLHH,MLHH2,“po0 #”,MLHV,MLHV2,“po0 #”,MLK,MLK2,“po0 #”, |
FLHH,FLHH2,“po0 #”,FLHV,FLHV2,“po0 #”,FLK,FLK2,“po0”,13]
;SSC-32 -> H3 engine
gosub H3Init
Sound 9,[100\4235, 200\4435]
;--------------------------------------------------------------------
;-------------Main loop
main
if fNewCommand then
; Ok we received a new command by our IRQ0 interrupt
; so lets transfer it into the main command buffer
PMR1.bit4 = 0; ; Disable interrupt to keep command integrety…
for Index = 0 to 6
DualShock(Index) = abNewCommand(Index)
next
PMR1.bit4 = 1; ; Done looking at command so reenable…
; BUGBUG: set a maximum number of times a command can repeat
; for button presses I set to 1, else set to 255, maybe should
; change command between host and here to allow them to tell us...
if DualShock(1) <> $FF or DualShock(2) <> $FF then
nRepeatCommand = 1
else
nRepeatCommand = 255
endif
elseif nRepeatCommand > 0
nRepeatCommand = nRepeatCommand - 1
if nRepeatCommand = 0 then
; we did the command the max number of times so go back to neutral...
DualShock(1) = $FF
DualShock(2) = $FF
for Index = 3 to 6
DualShock(Index) = $80
next
endif
endif
;**********************************************************************
; R1-L1-R2-L2 presets code
;
; to change Body Height (range is -25 (body up) to 25 (body down)) :
;Height = 10 ; for example
;
; to change Leg Up Shift (range is 20 (close to floor) to 70 (about 2.5 inches from floor))
;LegUpShift = 30 ; for example
;
; to change Gait Speed (range is 3 (fast) to 20 (very slow))
;GaitSpeed = 4 ; for example
if (DualShock(2).bit3 = 0) and LastButton(1).bit3 then ;R1 Button test
Height = -25 ; tall grass
LegUpShift = 70
GaitSpeed = 10
elseif (DualShock(2).bit2 = 0) and LastButton(1).bit2 ;L1 Button test
Height = 5 ; tile floor
LegUpShift = 30
GaitSpeed = 3
elseif (DualShock(2).bit1 = 0) and LastButton(1).bit1 ;R2 Button test
Height = 25 ; Body low
LegUpShift = 35
GaitSpeed = 4
elseif (DualShock(2).bit0 = 0) and LastButton(1).bit0 ;L2 Button test
Height = 0 ; Default preset
LegUpShift = 35
GaitSpeed = 4
else
goto NoPresets
endif
HeightAdjust = 0
LegUpShift = (LegUpShift max LegUpShiftMax) min LegUpShiftMin
GaitKind = LegUpShift > 45
GaitSpeedTmp = GaitSpeed max 20
MovesDelay = 8
NoPresets
;**********************************************************************
if (DualShock(2).bit4 = 0) and LastButton(1).bit4 then ;Triangle Button test
FlagOff = FlagOff ^ 1
if FlagOff then
gosub All1500
Gosub InitPos
pause 144
for Index = 0 to 31
serout SSC32,i38400,"#",DEC Index,"P0"]
next
serout SSC32,i38400,[13]
else
gosub H3Init
endif
endif
if FlagOff then
pause 144
goto OffEnd
endif
if (MovesDelay <= 0) or LockLegs then
if (DualShock(2).bit5 = 0) and LastButton(1).bit5 then ;Circle Button test
gosub All1500
endif
if (DualShock(2).bit6 = 0) and LastButton(1).bit6 then ;Cross Button test
gosub All1500
;Attack posture
serout SSC32,i38400,"#",MRHH,MRHH2,"P1800#",MRHV,MRHV2,"P1700#",MRK,MRK2,"P1700#", |
MLHH,MLHH2,"P1200#",MLHV,MLHV2,"P1300#",MLK,MLK2,"P1300T288",13]
pause 288
serout SSC32,i38400,"#",MRHV,MRHV2,"P1500#",MRK,MRK2,"P1500#",MLHV,MLHV2,"P1500#", |
MLK,MLK2,"P1500T288",13]
pause 288
serout SSC32,i38400,"#",RRHV,RRHV2,"P1800#",RRK,RRK2,"P1800#",MRHV,MRHV2,"P1400#", |
MRK,MRK2,"P1400#",FRHH,FRHH2,"P1800#",FRHV,FRHV2,"P2100#",RLHV,RLHV2,"P1200#", |
RLK,RLK2,"P1200#",MLHV,MLHV2,"P1600#",MLK,MLK2,"P1600#",FLHH,FLHH2,"P1200#", |
FLHV,FLHV2,"P900T288",13]
pause 288
for Index = 1 to 10
serout SSC32,i38400,"#",FRHV,FRHV2,"P2000#",FRK,FRK2,"P1600#",FLHV,FLHV2,"P900#", |
FLK,FLK2,"P1600T144",13]
pause 144
serout SSC32,i38400,"#",FRHV,FRHV2,"P2100#",FRK,FRK2,"P1400#",FLHV,FLHV2,"P1000#", |
FLK,FLK2,"P1400T144",13]
pause 144
next
serout SSC32,i38400,"#",FRHH,FRHH2,"P1500#",FRK,FRK2,"P1500#",FLHH,FLHH2,"P1500#", |
FLHV,FLHV2,"P900#",FLK,FLK2,"P1500T288",13]
pause 288
serout SSC32,i38400,"#",RRHV,RRHV2,"P1500#",RRK,RRK2,"P1500#",MRHV,MRHV2,"P1500#", |
MRK,MRK2,"P1500#",FRHV,FRHV2,"P1500#",MLHV,MLHV2,"P1500#",MLK,MLK2,"P1500#", |
FLHV,FLHV2,"P1500#",RLHV,RLHV2,"P1500#",RLK,RLK2,"P1500T576",13]
pause 576
serout SSC32,i38400,"#",MRHH,MRHH2,"P1500#",MRHV,MRHV2,"P1700#",MRK,MRK2,"P1700#", |
MLHH,MLHH2,"P1500#",MLHV,MLHV2,"P1300#",MLK,MLK2,"P1300T288",13]
pause 288
serout SSC32,i38400,"#",MRHV,MRHV2,"P1500#",MRK,MRK2,"P1500#",MLHV,MLHV2,"P1500#", |
MLK,MLK2,"P1500T288",13]
pause 288
endif
if (DualShock(2).bit7 = 0) and LastButton(1).bit7 then ;Square Button test
gosub All1500
;Learning to fly posture (uh ? nutty ?...it CAN fly...)
for Index = 1 to 4
serout SSC32,i38400,"#",MRHV,MRHV2,"P1900#",MRK,MRK2,"P1800#",MLHV,MLHV2,"P1100#", |
MLK,MLK2,"P1200T288",13]
serout SSC32,i38400,"#",RRHV,RRHV2,"P1600#",RRK,RRK2,"P1600#",FRHV,FRHV2,"P1600#", |
FRK,FRK2,"P1600#",RLHV,RLHV2,"P1400#",RLK,RLK2,"P1400#",FLHV,FLHV2,"P1400#", |
FLK,FLK2,"P1400T576",13]
pause 288
serout SSC32,i38400,"#",MRK,MRK2,"P1200#",MLK,MLK2,"P1800T288",13]
pause 288
serout SSC32,i38400,"#",RRHV,RRHV2,"P1400#",RRK,RRK2,"P1400#",MRHV,MRHV2,"P1500#", |
FRHV,FRHV2,"P1400#",FRK,FRK2,"P1400#",RLHV,RLHV2,"P1600#",RLK,RLK2,"P1600#", |
MLHV,MLHV2,"P1500#",FLHV,FLHV2,"P1600#",FLK,FLK2,"P1600T576",13]
pause 576
next
serout SSC32,i38400,"#",MRHV,MRHV2,"P1800#",MRK,MRK2,"P1800#",MLHV,MLHV2,"P1200#", |
MLK,MLK2,"P1200T288",13]
pause 288
gosub All1500
endif
endif
if (DualShock(1).bit0 = 0) and LastButton(0).bit0 then ;Select Button test
LockLegs = LockLegs ^ 1
Sound 9,[100\(2000 + LockLegs * 1000)]
gosub FlatBody
endif
if (DualShock(1).bit3 = 0) and LastButton(0).bit3 then ;Start Button test
KneeShift(6) = KneeShift(6) ^ KneeShiftPulse
Sound 9,[100\(800 + KneeShift(6))]
endif
if (DualShock(1).bit1 = 0) and LastButton(0).bit1 then ;L3 Button test
if LockLegs then
LeftStickMode = (LeftStickMode + 1) & 3
gosub FlatBody
else
HeightAdjust = HeightAdjust ^ 1
endif
Sound 9,[100\3500]
endif
if (DualShock(1).bit2 = 0) and LastButton(0).bit2 then ;R3 Button test (Horn)
Sound 9,[800\392, 600\370, 200\0, 800\330, 600\294, 200\0, 300\330, 100\0, 300\330]
endif
if DualShock(1).bit4 = 0 then ;Up Button test
LegUpShift = (LegUpShift + 1) max LegUpShiftMax
elseif DualShock(1).bit6 = 0 ;Down Button test
LegUpShift = (LegUpShift - 1) min LegUpShiftMin
else
goto NoSound
endif
GaitKind = LegUpShift > 45
Sound 9,[20\(LegUpShift * 10 + 400 + GaitKind * 200)]
MovesDelay = 8
NoSound
if (DualShock(1).bit5 = 0) and LastButton(0).bit5 then ;Right Button test
GaitSpeedTmp = (GaitSpeedTmp + 1) max 20
elseif (DualShock(1).bit7 = 0) and LastButton(0).bit7 ;Left Button test
GaitSpeedTmp = (GaitSpeedTmp - 1) min 3
else
goto NoSound1
endif
Sound 9,[20\(50-GaitSpeedTmp) * 35 + 400]
MovesDelay = 8
NoSound1
GaitSpeed = GaitSpeedTmp min (3 + GaitKind)
for Index = 3 to 6
if DualShock(Index) > (128 + DeadZone) then
DualShock(Index) = DualShock(Index) - DeadZone/2
elseif DualShock(Index) < (128 - DeadZone)
DualShock(Index) = DualShock(Index) + DeadZone/2
else
DualShock(Index) = 128
endif
next
YSpeed = DualShock(3) - 128 ; Right Stick Horizontal
XSpeed = DualShock(4) - 128 ; Right Stick Vertical
Steering = DualShock(5) - 128 ; Left Stick Horizontal
StepFlag = (GaitSpeedTmp - 2) max 3
if HeightAdjust or LockLegs then
Height = LastHeight + (((DualShock(6) - 128) / 5) - LastHeight) / StepFlag; Left Stick Vertical
endif
Height = (Height Max HeightLimit) min - HeightLimit
;H3
if Steering or (Height <> LastHeight) or YSpeed or XSpeed or LockLegs then
MovesDelay = 8
endif
if MovesDelay > 0 then
LastHeight = Height
DCoord = SQR(XSpeed * XSpeed + YSpeed * YSpeed)
TmpCos = XSpeed * 127 / DCoord
gosub ACos
DAngle = TmpAngle
if YSpeed > 0 then
DAngle = 256 - DAngle
endif
DCoord = DCoord max (128 - ABS(Height) - ABS(TibiaAngle * 2))
; 180° Deck Code
if (Dangle < 64 or Dangle > 191) and (DCoord > 0) then
DeckTilt_Pulse = 1680 - Dcoord
else
DeckTilt_Pulse = 1380 + Dcoord
endif
Deck_Pulse = ((Deck_PulseMax - Deck_PulseMin) * ((DAngle + 64 ) & $7F)) / 127 + Deck_PulseMin
; 360° Deck Code
;Deck_Pulse = ((Deck_PulseMax - Deck_PulseMin) * DAngle) / 255 + Deck_PulseMin
Steps = Steps + 1
if Steps > NbSteps then
Steps = 1
FlipFlap = FlipFlap ^ 1
NbSteps = GaitSpeed
endif
if LockLegs then
if LeftStickMode > 0 then ;Body roll and pitch
if (leftStickMode & 1) then
Roll = Roll + (Steering - Roll) / StepFlag
Steering = 0
else
Roll = 0
endif
if (leftStickMode & 2) then
Pitch = Pitch + (Height - Pitch) / StepFlag
Height = 0
else
Pitch = 0
endif
HeightShift(0) = Roll/10
HeightShift(2) = HeightShift(0)
HeightShift(3) = -HeightShift(0)
HeightShift(5) = HeightShift(3)
HeightShift(0) = ((HeightShift(0) + Pitch) max HeightLimit) min -HeightLimit
HeightShift(3) = -HeightShift(0)
HeightShift(2) = ((HeightShift(2) - Pitch) max HeightLimit) min -HeightLimit
HeightShift(5) = -HeightShift(2)
HeightShift(1) = HeightShift(0) + HeightShift(2)
HeightShift(4) = HeightShift(3) + HeightShift(5)
endif
LittleGripOCPulse = ((LittleGripOCPulse + (DualShock(2).bit3 * 50) - (DualShock(2).bit2 * 50)) |
min LGripOC_PulseMin) max LGripOC_PulseMax
LittleGripLRPulse = ((LittleGripLRPulse + (DualShock(2).bit1 * 50) - (DualShock(2).bit0 * 50)) |
min LGripLR_PulseMin) max LGripLR_PulseMax
GaitSpeedTmp = StepFlag + 2
else
StepFlag = NbSteps - Steps + 1
endif
for Index = 0 to 5
XPos2(Index) = -(DCoord * SIN(DAngle + (Index * 43 + 21) + 64) / 300) ; 43 => 60 degrees
ZPos2(Index) = -(DCoord * SIN(DAngle + (Index * 43 + 21)) / 300) ; 43 => 60 degrees
next
Tripod = FlipFlap
gosub AorB_Down
Freeze = Steps < (NbSteps - GaitKind)
Tripod = FlipFlap ^ 1
gosub AorB_Up
for Index = 0 to 5
;Distance and HipH_Angle with XZ
Distance = SQR((XPos(Index) * XPos(Index) + ZPos(Index) * ZPos(Index)) / 4) * 2
TmpCos = ZPos(Index) * 127 / Distance
Distance = Distance + TibiaAngle ; Tibia angle for special legs (3DOF-C)
Gosub ACos
HipH_Angle = (TmpAngle max HipH_AngleMax) min HipH_AngleMin
;----------
;Set Angle
TmpXPos = XPos(Index) * 127
TmpYPos = (YPos(Index) * 127) max 0
TmpZPos = ZPos(Index) * 127
TmpDistance = ((Distance - HipV_HipH) * 127) min 0
TmpSEWSEW = (TmpYPos * TmpYPos + TmpDistance * TmpDistance) / 16129
TmpSEW = SQR(TmpSEWSEW)
TmpCos = (Femur_Length * Femur_Length + Tibia_Length * Tibia_Length - TmpSEWSEW)
TmpCos = TmpCos * 127 / (2 * Femur_Length * Tibia_Length)
gosub ACos
Knee_Angle = (TmpAngle max Knee_AngleMax) min Knee_AngleMin
TmpCos = -TmpYPos / TmpSew
gosub ACos
HipV_Angle = TmpAngle
TmpCos = (Femur_Length * Femur_Length - Tibia_Length * Tibia_Length + TmpSEWSEW)
TmpCos = TmpCos * 127 / (2 * Femur_Length * TmpSEW)
gosub ACos
HipV_Angle = ((TmpAngle + HipV_Angle) max HipV_AngleMax) min HipV_AngleMin
;----------
HipH_Angle = HipH_Angle + Rotate(Index)
HipH_Pulse(Index) = (((HipH_PulseMax - HipH_PulseMin) * (HipH_Angle - HipH_AngleMin) |
/ (HipH_AngleMax - HipH_AngleMin) + HipH_PulseMin) max HipH_PulseMax) min HipH_PulseMin
HipV_Pulse(Index) = (((HipV_PulseMax - HipV_PulseMin) * (HipV_Angle - HipV_AngleMin) |
/ (HipV_AngleMax - HipV_AngleMin) + HipV_PulseMin) max HipV_PulseMax) min HipV_PulseMin
Knee_Pulse(Index) = (((Knee_PulseMax - Knee_PulseMin) * (Knee_Angle - Knee_AngleMin) |
/ (Knee_AngleMax - Knee_AngleMin) + Knee_PulseMin) max Knee_PulseMax) min Knee_PulseMin
Knee_Pulse(Index) = 3000 - Knee_Pulse(Index) - KneeShift(Index) * 2
next
serout SSC32,i38400,"#",RRHH,RRHH2,"P",DEC HipH_Pulse(0),"#",RRHV,RRHV2,"P",DEC HipV_Pulse(0), |
"#",RRK,RRK2,"P",DEC Knee_Pulse(0),"#",FRHH,FRHH2,"P",DEC HipH_Pulse(2), |
"#",FRHV,FRHV2,"P",DEC HipV_Pulse(2),"#",FRK,FRK2,"P",DEC Knee_Pulse(2), |
"#",MLHH,MLHH2,"P",DEC HipH_Pulse(4),"#",MLHV,MLHV2,"P",DEC 3000 - HipV_Pulse(4), |
"#",MLK,MLK2,"P",DEC 3000 - Knee_Pulse(4),"#",MRHH,MRHH2,"P",DEC HipH_Pulse(1), |
"#",MRHV,MRHV2,"P",DEC HipV_Pulse(1),"#",MRK,MRK2,"P",DEC Knee_Pulse(1), |
"#",RLHH,RLHH2,"P",DEC HipH_Pulse(5),"#",RLHV,RLHV2,"P",DEC 3000 - HipV_Pulse(5), |
"#",RLK,RLK2,"P",DEC 3000 - Knee_Pulse(5),"#",FLHH,FLHH2,"P",DEC HipH_Pulse(3), |
"#",FLHV,FLHV2,"P",DEC 3000 - HipV_Pulse(3),"#",FLK,FLK2,"P",DEC 3000 - Knee_Pulse(3), |
"#",DeckP,DeckP2,"P",DEC Deck_Pulse,"#",DeckTilt,DeckTilt2,"P",DEC DeckTilt_Pulse, |
"#",LGripOC,LGripOC2,"P",DEC LittleGripOCPulse,"#",LGripLR,LGripLR2,"P",DEC LittleGripLRPulse,"T210",13]
else
pause 144
if MovesDelay < -15 then
MovesDelay = (NbSteps - GaitKind) - Steps + 1
EndIf
endif
MovesDelay = MovesDelay - 1
OffEnd
LastButton(0) = DualShock(1)
LastButton(1) = DualShock(2)
goto main
;--------------------------------------------------------------------
;-------------Sub H3 Init
H3Init
LockLegs = 0
NbSteps = 4
GaitSpeedTmp = NbSteps
LegUpShift = 35
for Index = 0 to 5
XPos(Index) = HipV_HipH + Femur_Length
YPos(Index) = - Tibia_Length
ZPos(Index) = 0
next
Gosub InitPos
MovesDelay = 8
; then All1500
;-------------Sub All1500
All1500
HeightAdjust = 1
height = 0
LastHeight = 0
for Index = 0 to 27 ;preserve servo 28 (Deck Tilt),29 and 30 (Little Gripper),31 (Deck)
serout SSC32,i38400,"#",DEC Index,"P1500"]
next
serout SSC32,i38400,"T576",13]
pause 576
Steps = NbSteps
; then Flat Body
;-------------Sub Flat Body
FlatBody
Roll = 0
Pitch = 0
for Index = 0 to 5
HeightShift(Index) = 0
next
return
;-------------Sub InitPos
InitPos
serout SSC32,i38400,"#",RRHV,RRHV2,“P2100#”,RRK,RRK2,“P1800#”,MRHV,MRHV2,“P2100#”,MRK,MRK2,“P1800#”, |
FRHV,FRHV2,“P2100#”,FRK,FRK2,“P1800#”,RLHV,RLHV2,“P900#”,RLK,RLK2,“P1200#”,MLHV,MLHV2,“P900#”, |
MLK,MLK2,“P1200#”,FLHV,FLHV2,“P900#”,FLK,FLK2,“P1200T1152”,13]
pause 576
serout SSC32,i38400,"#",RRHH,RRHH2,“P1500#”,MRHH,MRHH2,“P1500#”,FRHH,FRHH2,“P1500#”, |
RLHH,RLHH2,“P1500#”,MLHH,MLHH2,“P1500#”,FLHH,FLHH2,“P1500T576”,13]
pause 576
return
;-------------Sub Arc Cosinus
ACos
TmpCos = (TmpCos max 127) min -127
if TmpCos < 0 then
TmpAngle = 128 - ArcCos(-TmpCos)
else
TmpAngle = ArcCos(TmpCos)
endif
return
;-------------Tripod ‘A’ (tripod = 0) or ‘B’ (Tripod = 1) Down
AorB_Down
for Index = 0 + Tripod to 4 + Tripod Step 2
KneeShift(Index) = 0
Rotate(Index) = Rotate(Index) + (Steering/5 - Rotate(Index))/StepFlag
YPos(Index) = -Tibia_Length + Height + HeightShift(Index)
XPos(Index) = XPos(Index) + (XPos2(Index) - (XPos(Index) - (HipV_HipH + Femur_Length)))/StepFlag
ZPos(Index) = ZPos(Index) + (ZPos2(Index) - ZPos(Index))/StepFlag
next
return
;-------------Tripod ‘A’ (tripod = 0) or ‘B’ (Tripod = 1) Up
AorB_Up
if LockLegs then AorB_Down ;Goto sub AorB_Down and return directly from it to main loop
if (Steps = NbSteps) and GaitKind then
StepFlag = NbSteps
goto AorB_Down ;Goto sub AorB_Down and return directly from it to main loop
else
StepFlag = StepFlag - GaitKind - 1
endif
for Index = 0 + Tripod to 4 + Tripod Step 2
if (Steps > 1) and (Steps < (NbSteps - GaitKind - 1)) then
KneeShift(Index) = KneeShift(6)
else
KneeShift(Index) = 0
endif
Rotate(Index) = Rotate(Index) + (-Steering/5 - Rotate(Index))/StepFlag
YPos(Index) = -Tibia_Length + Height + (LegUpShift * Freeze)
XPos(Index) = XPos(Index) + (-XPos2(Index) - (XPos(Index) - (HipV_HipH + Femur_Length)))/StepFlag
ZPos(Index) = ZPos(Index) + (-ZPos2(Index) - ZPos(Index))/StepFlag
next
return
;--------------------------------------------------------------------
;==============================================================================
; We received an interrupt on IRQ0 so something is coming in on the S_IN/S_OUT line.
;
;==============================================================================
HANDLE_S_IN_INT:
; disable our own interrupt.
PMR1.bit4 = 0 ; restore pin n to normal IO
enable ; Allow other interrupts to be processed
;SerialQuery
serin S_OUT,i9600,256,NoData,[str abNewCommand(0)\7]
CheckSum = abNewCommand(1) ^ abNewCommand(2) ^ abNewCommand(3) ^ |
abNewCommand(4) ^ abNewCommand(5) ^ abNewCommand(6)
if CheckSum = abNewCommand(0) then
fNewCommand = 1 ; let the main loop know we have a new command
else
NoData:
; We might do something different here if we always want to process a command.
;
; PMR.bit4 = 1; ; in an error reenable the interrupt…
endif
; Decided to allow commands to overwrite the previous command even if we have not processed it yet…
; Last one wins. Could do differently and don’t enable again until we process a command.
PMR1.bit4 = 1; ; reenable the interrupt…
resume ; and return
[/code]
