I am having a slight issue with the programming on my EH2.
I am using the supplied programs from the tutorial, but om programs 4 and 5 there appears to be an issue with the adin function, and the compiler always picks up an error there. The error is a unknown token type error, [TOKEN ,] is the display.
I am currently working off of a Basic ATOM Pro 28 pin chip, if that matters any.
Any help with this issue would be greatly appreciated.
It would help to have additional information.
I am not sure what programs 4 and 5 are. It would help if your post included: the actual source line that failed. My guess is that the adin is refering to the wrong pins or is using the pin names for an Atom not a pro.
On the Atom Pro the pins should either be the actual pin number or the pin names: P0-p3 and p16-p19.
On the atom, I believe they are labeled: AX0, AX1, AX2 and AX3.
Good Luck
It is written for the Atom, not the pro. Here is the tutorial. We still need to change it over to the Bot Board II, but there is a bigger change that will probably happen first.
kurte,
In the link above the programs are h2prog4 and h2prog5, the compiler finds the errors at line 362 for 4, which reads:
adin AX0,2,AD_RON,GP2D12
and line 477 for 5, which reads:
adin AX0,2,AD_RON,GP2D12
ok working off of an Atom 28 now, only small issues with the programming. The arm controls are not what they should be, the gripper will open and close with the left stick, the motion controls appear to be skewed, and the sensor is not working, i.e. beeping/vibrating. attached is the code:
;************************************************
;*** Basic Atom with SSC-32 and PS2 DualShock ***
;******** or MadCatz wireless controller ********
;* H2 program #5 : Tank move + 1 joystick move *
;***** H2 legs low, middle and high control *****
;******* Sharp GP2D12 IR Sensor Support *********
;************** L5-L6 Arm Support ***************
;********** Bot Board Buzzer support ************
;************************************************
;** Programmer: Laurent Gay, [email protected] **
;************************************************
;
; H2 Control Mode 1 :
; Control the right legs with the right joystick (SSC-32 XR command)
; Control the left legs with the left joystick (SSC-32 XL command)
; the global speed is mixed (SSC-32 XS command)
;
; Push the R3 button (Right joystick push button) to swap between control mode
;
; H2 Control Mode 2 :
; Control All legs with the right joystick (SSC-32 XR-XL command mixed)
; the global speed is mixed too (SSC-32 XS command)
;
; Right and Left buttons adjust the speed limit (Mode 1 & 2)
;
; Control The H2 legs ‘high’ position with the R1/R2 buttons
; Control The H2 legs ‘low’ position with the L1/L2 buttons (H2 body height)
; Control The H2 legs ‘low’ position with the Up/Down buttons (H2 body height, 10 times faster than L2/R2)
; and push High position to max (when Up button pressed) if needed
; change the legs trajectories Triangle button -> triangle, Circle button -> triangle like,
; Cross button -> Square like, Square button -> square
;
; Auto avoid obstacles with the Sharp GP2D12 IR Sensor (lock forward) with sound and pad vibration proportional!
; Disable/enable the sensor with the Start button
;
; Swap between H2 and Arm Control with Select button
;
; Initialize Arm position with the Cross button
; Disable/enable Arm servos with the Triangle button
; Invert the X axis with the L3 button (Left joystick push button)
; Close/Open grip with L1/L2 buttons
; Turn wrist (L6 only) Right/Left with R1/R2 buttons
; move X-Y with left joystick
; move Z and Wrist angle with right joystick
;
; you may have to push the Analog Button on a MadCatz Wireless controller (if in sleep mode)
;
;************************************************
;
;
;--------------------------------------------------------------------
;-------------Constants
;PS2 Controller / BotBoard I
DAT con P4
CMD con P5
SEL con P6
CLK con P7
SSC32 con p15
;PS2 Controller / BotBoard II
;DAT con P12
;CMD con P13
;SEL con P14
;CLK con P15
;SSC32 con p8
DeadZone con 28 ; must be >= 28
AxisSpeed con 15
PadMode con $79 ; Dualshock2 mode, use $73 with a Dualshock1
;Sharp GP2D12 IR sensor
GP2D12_Threshold con 250
GP2D12_SoundThreshold con 112
;H2
H2_RH con 2000 ;assume LH = (3000 - H2_RH) = 1000 !
H2_RM con 1333 ;assume LM = (3000 - H2_RM) = 1667 !
H2_RL con 1000 ;assume LL = (3000 - H2_RL) = 2000 !
;Arm
BasePin con 8
Base_PulseDef con 1500
Base_PulseMin con 1300
Base_PulseMax con 1700
ShoulderPin con 9
Shoulder_AngleDef con 110 ;155° (155/360*256)
Shoulder_AngleMin con 0 ;0°
Shoulder_AngleMax con 110 ;155°
Shoulder_PulseMin con 700
Shoulder_PulseMax con 2050
ElbowPin con 24
Elbow_AngleDef con 110 ;155° (155/360*256)
Elbow_AngleMin con 0 ;0°
Elbow_AngleMax con 110 ;155°
Elbow_PulseMin con 640
Elbow_PulseMax con 2150
WristPin con 25
Wrist_AngleShift con -64 ;Wrist-Forearm Shift
Wrist_AngleDef con 46 ;65° (65/360*256)
Wrist_AngleMin con 0 ;0°
Wrist_AngleMax con 128 ;180°
Wrist_PulseMin con 600
Wrist_PulseMax con 2370
GripPin con 26
Grip_PulseDef con 1500
Grip_PulseMin con 1000
Grip_PulseMax con 1800
WristRotPin con 27
WristRot_PulseDef con 1500
WristRot_PulseMin con 750
WristRot_PulseMax con 2250
;L6
;Arm_Length con 121 ;4.75" = 121mm (4.75 * 25.4)
;Forearm_Length con 121 ;4.75" = 121mm (4.75 * 25.4)
;Hand_Length con 146 ;5.75" = 146mm (5.75 * 25.4)
;L5
Arm_Length con 95 ;3.75" = 95mm (3.75 * 25.4)
Forearm_Length con 95 ;3.75" = 95mm (3.75 * 25.4)
Hand_Length con 127 ;5.00" = 146mm (5.00 * 25.4)
;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,08,06,04,02 ;Cheating on values (vertical lag issue)
;20,19,19,18,17,16,15,15,14,13,11,10,09,07,05,00 ;Real values
;--------------------------------------------------------------------
;-------------Variables
index var Byte
DualShock var Byte(7)
LastButton var Byte(2)
XR var SWord
XL var SWord
CmdMode var Bit ;1 = H2, 0 = Arm
MoveModeH2 var Bit ;1 = One Joystick, 0 = Tank
ParkArm var Bit
NeedServosOn var Bit
GP2D12Enable var Bit
ArmDirection var Bit ;1 = Left to right, 0 = right to left
ArmOnOff var Bit
LockForward var Bit
LargeMotor var Byte
Speed var Byte
MaxSpeed var Byte
MiddleMode var Nib ;0 = triangle, 1 = nearly triangle, 2 = nearly square, 3 = square
H2_High var Word
H2_Middle var Word
H2_Low var word
GP2D12 var Word
Time var Word
XCoord var Sbyte
YCoord var Sbyte
ZCoord var Sbyte
WCoord var Sbyte
TmpYPos var Long
TmpDistance var Long
TmpSEW var Word
TmpSEWSEW var Long
TmpCos var Long
TmpAngle var SWord
Distance var Sword
YPos var Sword
Wrist_TableAngle var SWord
Shoulder_Angle var Sword
Elbow_Angle var Sword
Wrist_Angle var Sword
Base_Pulse var word
Shoulder_Pulse var word
Elbow_Pulse var word
Wrist_Pulse var word
Grip_Pulse var word
WristRot_Pulse var word
;--------------------------------------------------------------------
;***************
;*** Program ***
;***************
;-------------Init
;DualShock
pause 500
clear
high CLK
again
low SEL
shiftout CMD,CLK,FASTLSBPRE,$1\8,$43\8,$0\8,$1\8,$0\8] ;CONFIG_MODE_ENTER
high SEL
pause 1
low SEL
shiftout CMD,CLK,FASTLSBPRE,$01\8,$44\8,$00\8,$01\8,$03\8,$00\8,$00\8,$00\8,$00\8] ;SET_MODE_AND_LOCK
high SEL
pause 100
low SEL
shiftout CMD,CLK,FASTLSBPRE,$01\8,$4F\8,$00\8,$FF\8,$FF\8,$03\8,$00\8,$00\8,$00\8] ;SET_DS2_NATIVE_MODE
high SEL
pause 1
low SEL
shiftout CMD,CLK,FASTLSBPRE,$01\8,$4D\8,$00\8,$00\8,$01\8,$FF\8,$FF\8,$FF\8,$FF\8] ;VIBRATION_ENABLE
high SEL
pause 1
low SEL
shiftout CMD,CLK,FASTLSBPRE,$01\8,$43\8,$00\8,$00\8,$5A\8,$5A\8,$5A\8,$5A\8,$5A\8] ;CONFIG_MODE_EXIT_DS2_NATIVE
high SEL
pause 1
low SEL
shiftout CMD,CLK,FASTLSBPRE,$01\8,$43\8,$00\8,$00\8,$00\8,$00\8,$00\8,$00\8,$00\8] ;CONFIG_MODE_EXIT
high SEL
pause 1
low SEL
shiftout CMD,CLK,FASTLSBPRE,$1\8]
shiftin DAT,CLK,FASTLSBPOST,[DualShock(0)\8]
high SEL
pause 1
;serout S_OUT,i57600,"PadMode : ",HEX2 DualShock(0),13]
Sound 9,[100\4435]
if DualShock(0) <> PadMode then again
CmdMode = 1 ;1 = H2, 0 = Arm
MoveModeH2 = 0 ;1 = One Joystick, 0 = Tank
GP2D12Enable = 1
ArmDirection = 1 ;1 = Left to right, 0 = Right to left
ArmOnOff = 1
LockForward = 0
LargeMotor = 0
MiddleMode = 1
MaxSpeed = 100
LastButton(0) = 255
LastButton(1) = 255
;Arm engine
ParkArm = 0
NeedServosOn = 1
gosub ArmInit
;SSC-32 -> H2 engine
pause 500
gosub H2Init
;--------------------------------------------------------------------
;-------------Main loop
main
;DS2
low SEL
shiftout CMD,CLK,FASTLSBPRE,$1\8,$42\8]
for index = 0 to 2
shiftin DAT,CLK,FASTLSBPOST,[DualShock(index)\8]
next
high SEL
pause 1
low SEL
shiftout CMD,CLK,FASTLSBPRE,$1\8,$42\8,$0\8,$0\8,LargeMotor\8]
for index = 3 to 6
shiftin DAT,CLK,FASTLSBPOST,[DualShock(index)\8]
next
high SEL
pause 1
XCoord = DualShock(5) - 128
if XCoord > DeadZone then
XCoord = XCoord - DeadZone
elseif XCoord < -DeadZone
XCoord = XCoord + DeadZone
else
XCoord = 0
endif
YCoord = DualShock(6) - 128
if YCoord > DeadZone then
YCoord = YCoord - DeadZone
elseif YCoord < -DeadZone
YCoord = YCoord + DeadZone
else
YCoord = 0
endif
ZCoord = DualShock(3) - 128
if ZCoord > DeadZone then
ZCoord = ZCoord - DeadZone
elseif ZCoord < -DeadZone
ZCoord = ZCoord + DeadZone
else
ZCoord = 0
endif
WCoord = DualShock(4) - 128
if WCoord > DeadZone then
WCoord = WCoord - DeadZone
elseif WCoord < -DeadZone
WCoord = WCoord + DeadZone
else
WCoord = 0
endif
if (DualShock(1).bit0 = 0) and LastButton(0).bit0 then ;Select Button test
CmdMode = CmdMode ^ 1
if CmdMode then
Sound 9,[50\440,25\1,50\440]
else
serout SSC32,i38400,"XSTOP",13]
serout SSC32,i38400,"#0P",DEC H2_RL," #2P",DEC H2_RL," #4P",DEC H2_RL |
," #16P",DEC 3000 - H2_RL," #18P",DEC 3000 - H2_RL," #20P",DEC 3000 - H2_RL, " S1000",13]
ArmOnOff = 1
LargeMotor = 0
Sound 9,[50\1760,25\1,50\1760]
endif
endif
if CmdMode then
;H2
if (DualShock(2).bit4 = 0) and LastButton(1).bit4 then ;Triangle Button test
MiddleMode = 4
elseif (DualShock(2).bit5 = 0) and LastButton(1).bit5 ;Circle Button test
MiddleMode = 5
elseif (DualShock(2).bit6 = 0) and LastButton(1).bit6 ;Cross Button test
MiddleMode = 6
elseif (DualShock(2).bit7 = 0) and LastButton(1).bit7 ;Square Button test
MiddleMode = 7
else
goto NoSound1
endif
Sound 9,[100\1318]
NoSound1
if (DualShock(2).bit2 = 0) and (H2_Low >= (H2_RL + 20)) then ;L1 Button test
H2_Low = H2_Low - 20
MiddleMode = MiddleMode | 8
elseif (DualShock(2).bit0 = 0) and (H2_Low <= (H2_High - 20)) ;L2 Button test
H2_Low = H2_Low + 20
MiddleMode = MiddleMode | 8
else
goto NoSound2
endif
Sound 9,[30((H2_RH - H2_Low) / 2 + 100)]
NoSound2
if DualShock(1).bit4 = 0 then ;Up Button test
H2_Low = H2_Low - 200
if H2_Low < H2_RL then
H2_Low = H2_RL
goto NoSound3
endif
MiddleMode = MiddleMode | 8
elseif DualShock(1).bit6 = 0 ;Down Button test
H2_Low = H2_Low + 200
if H2_Low > H2_RH then
H2_Low = H2_RH
goto NoSound3
endif
H2_High = H2_High + 200
if H2_High > H2_RH then
H2_High = H2_RH
endif
MiddleMode = MiddleMode | 8
else
goto NoSound3
endif
Sound 9,[50((H2_RH - H2_Low) / 2 + 100)]
NoSound3
if (DualShock(2).bit1 = 0) and (H2_High >= (H2_Low + 20)) then ;R2 Button test
H2_High = H2_High - 20
MiddleMode = MiddleMode | 8
elseif (DualShock(2).bit3 = 0) and (H2_High <= (H2_RH - 20)) ;R1 Button test
H2_High = H2_High + 20
MiddleMode = MiddleMode | 8
else
goto NoSound4
endif
Sound 9,[30((H2_High - H2_RL) / 2 + 100)]
NoSound4
if (DualShock(1).bit2 = 0) and LastButton(0).bit2 then ;R3 Button test
MoveModeH2 = MoveModeH2 ^ 1
Sound 9,[100\1318]
endif
if (DualShock(1).bit3 = 0) and LastButton(0).bit3 then ;Start Button test
GP2D12Enable = GP2D12Enable ^ 1
if GP2D12Enable then
Sound 9,[100\880,100\1480]
else
LockForward = 0
LargeMotor = 0
sound 9,[100\1480,100\880]
endif
endif
if (DualShock(1).bit5 = 0) and (MaxSpeed <= 190) then ;Right Button test
MaxSpeed = MaxSpeed + 10
elseif (DualShock(1).bit7 = 0) and (MaxSpeed >= 20) ;Left Button test
MaxSpeed = MaxSpeed - 10
else
goto NoSound5
endif
Sound 9,[100\(MaxSpeed * 10 + 100)]
NoSound5
if MoveModeH2 then ;One Joystick Mode
XR = -ZCoord - WCoord
XL = ZCoord - WCoord
if XR > 100 then
XR = 100
elseif XR < -100
XR = -100
endif
if XL > 100 then
XL = 100
elseif XL < -100
XL = -100
endif
if abs(WCoord) > abs(ZCoord) then
Speed = abs(WCoord) * MaxSpeed / 100
else
Speed = abs(ZCoord) * MaxSpeed / 100
endif
else ;Tank Mode
if abs(WCoord) > abs(YCoord) then
Speed = abs(WCoord) * MaxSpeed / 100
else
Speed = abs(YCoord) * MaxSpeed / 100
endif
XR = -WCoord
XL = -YCoord
endif
if LockForward then
if (XR > 0) then
XR = 0
endif
if (XL > 0) then
XL = 0
endif
endif
if MiddleMode > 3 then
MiddleMode = MiddleMode & 3
if MiddleMode = 0 then
H2_Middle = H2_Low
elseif MiddleMode = 1
H2_Middle = (H2_High - H2_Low) / 3 + H2_Low
elseif MiddleMode = 2
H2_Middle = (H2_High - H2_Low) * 2 / 3 + H2_Low
else
H2_Middle = H2_High
endif
serout SSC32,i38400,"LH",DEC 3000 - H2_High," LM",DEC 3000 - H2_Middle," LL",DEC 3000 - H2_Low,13]
serout SSC32,i38400,"RH",DEC H2_High," RM",DEC H2_Middle," RL",DEC H2_Low,13]
if Speed = 0 then
Speed = 200 ;Ensure legs updating
endif
endif
serout SSC32,i38400,"XS",DEC Speed," XR",SDEC XR," XL",SDEC XL,13]
if GP2D12Enable then
adin AX0,2,AD_RON,GP2D12
LockForward = 0
LargeMotor = 0
if GP2D12 > GP2D12_Threshold then
LockForward = 1
LargeMotor = 255
Sound 9,[20\200]
elseif GP2D12 > GP2D12_SoundThreshold
LargeMotor = GP2D12
Sound 9,[10\(GP2D12 * 10)]
endif
endif
nap 2 ;internal sleep mode, approx 38ms, use value 3 or 4 with some slow wireless controller
else
;Arm
if (DualShock(2).bit6 = 0) and LastButton(1).bit6 then ;Cross Button test
ParkArm = 1
elseif (DualShock(2).bit4 = 0) and LastButton(1).bit4 ;Triangle Button test
ArmOnOff = ArmOnOff ^ 1
if ArmOnOff then
gosub ArmUpdatePosition
Sound 9,[100\523,100\659,100\783]
ParkArm = 0
NeedServosOn = 1
else
serout SSC32,i38400,"#",DEC BasePin,“P0 #”,DEC ShoulderPin,“P0 #”, |
DEC ElbowPin,“P0 #”,DEC WristPin,“P0 #”,DEC GripPin,“P0 #”,DEC WristRotPin,“P0”,13]
Sound 9,[100\783,100\659,100\523]
endif
endif
if ArmOnOff = 0 then ShortCut2
if (DualShock(1).bit1 = 0) and LastButton(0).bit1 then ;L3 Button test
ArmDirection = ArmDirection ^ 1
Sound 9,[100\1318]
endif
if XCoord or YCoord or WCoord or ParkArm then
if ParkArm then
gosub ArmInit
else
if WCoord then
Wrist_Angle = Wrist_Angle - (WCoord / (AxisSpeed * 2))
if Wrist_Angle > Wrist_AngleMax then
Wrist_Angle = Wrist_AngleMax
elseif Wrist_Angle < Wrist_AngleMin
Wrist_Angle = Wrist_AngleMin
endif
gosub GetPos
if TmpDistance > 0 then
Distance = TmpDistance / 127
YPos = TmpYPos / 127
Wrist_TableAngle = Elbow_Angle - Shoulder_Angle - Wrist_AngleShift - Wrist_Angle
else
Wrist_Angle = Wrist_Angle + (WCoord / (AxisSpeed * 2))
endif
endif
if ArmDirection then
Distance = Distance + (XCoord / AxisSpeed)
else
Distance = Distance - (XCoord / AxisSpeed)
endif
if Distance < 0 then
Distance = 0
endif
YPos = YPos - (YCoord / AxisSpeed)
gosub SetAngle
endif
Shoulder_Pulse = (Shoulder_PulseMax - Shoulder_PulseMin) * (Shoulder_Angle - Shoulder_AngleMin) |
/ (Shoulder_AngleMax - Shoulder_AngleMin) + Shoulder_PulseMin
if Shoulder_Pulse > Shoulder_PulseMax then
Shoulder_Pulse = Shoulder_PulseMax
elseif Shoulder_Pulse < Shoulder_PulseMin
Shoulder_Pulse = Shoulder_PulseMin
endif
Elbow_Pulse = (Elbow_PulseMax - Elbow_PulseMin) * (Elbow_Angle - Elbow_AngleMin) |
/ (Elbow_AngleMax - Elbow_AngleMin) + Elbow_PulseMin
if Elbow_Pulse > Elbow_PulseMax then
Elbow_Pulse = Elbow_PulseMax
elseif Elbow_Pulse < Elbow_PulseMin
Elbow_Pulse = Elbow_PulseMin
endif
Wrist_Pulse = (Wrist_PulseMax - Wrist_PulseMin) * (Wrist_Angle - Wrist_AngleMin) |
/ (Wrist_AngleMax - Wrist_AngleMin) + Wrist_PulseMin
if Wrist_Pulse > Wrist_PulseMax then
Wrist_Pulse = Wrist_PulseMax
elseif Wrist_Pulse < Wrist_PulseMin
Wrist_Pulse = Wrist_PulseMin
endif
gosub ArmUpdatePosition
endif
if ZCoord or NeedServosOn then
Base_Pulse = Base_Pulse + ZCoord / 8
if Base_Pulse > Base_PulseMax then
Base_Pulse = Base_PulseMax
elseif Base_Pulse < Base_PulseMin
Base_Pulse = Base_PulseMin
endif
serout SSC32,i38400,"#",DEC BasePin,"P",DEC Base_Pulse," T100",13]
endif
if DualShock(2).bit2 = 0 then ;L1 button test
Grip_Pulse = Grip_Pulse - 25
elseif DualShock(2).bit0 = 0 ;L2 button test
Grip_Pulse = Grip_Pulse + 25
elseif NeedServosOn = 0
goto Shortcut1
endif
if Grip_Pulse > Grip_PulseMax then
Grip_Pulse = Grip_PulseMax
elseif Grip_Pulse < Grip_PulseMin
Grip_Pulse = Grip_PulseMin
endif
serout SSC32,i38400,"#",DEC GripPin,"P",DEC Grip_Pulse," T100",13]
Shortcut1
if DualShock(2).bit3 = 0 then ;R1 button test
WristRot_Pulse = WristRot_Pulse - 25
elseif DualShock(2).bit1 = 0 ;R2 button test
WristRot_Pulse = WristRot_Pulse + 25
elseif NeedServosOn = 0
goto Shortcut2
endif
if WristRot_Pulse > WristRot_PulseMax then
WristRot_Pulse = WristRot_PulseMax
elseif WristRot_Pulse < WristRot_PulseMin
WristRot_Pulse = WristRot_PulseMin
endif
serout SSC32,i38400,"#",DEC WristRotPin,“P”,DEC WristRot_Pulse," T100",13]
NeedServosOn = 0
Shortcut2
nap 1 ;internal sleep mode, approx 19ms, use value 2 with some slow wireless controller
endif
LastButton(0) = DualShock(1)
LastButton(1) = DualShock(2)
goto main
;--------------------------------------------------------------------
;-------------Sub Update Position
ArmUpdatePosition
serout SSC32,i38400,"#",DEC ShoulderPin,“P”,DEC Shoulder_Pulse, |
" #",DEC ElbowPin,“P”,DEC Elbow_Pulse," #",DEC WristPin,“P”,DEC Wrist_Pulse," T",DEC Time,13]
Time = 150
return
;-------------Sub Arm Init
ArmInit
Shoulder_Angle = Shoulder_AngleDef
Elbow_Angle = Elbow_AngleDef
Wrist_Angle = Wrist_AngleDef
Base_Pulse = Base_PulseDef
Grip_Pulse = Grip_PulseDef
WristRot_Pulse = WristRot_PulseDef
gosub GetPos
Distance = TmpDistance / 127
YPos = TmpYPos / 127
Wrist_TableAngle = Elbow_Angle - Shoulder_Angle - Wrist_AngleShift - Wrist_Angle
if ParkArm then
serout SSC32,i38400,"#",DEC BasePin,"P",DEC Base_Pulse," S1000",13]
serout SSC32,i38400,"#",DEC GripPin,"P",DEC Grip_Pulse," S1000",13]
serout SSC32,i38400,"#",DEC WristRotPin,"P",DEC WristRot_Pulse," S1000",13]
ParkArm = 0
Time = 750
endif
ArmOnOff = 1
ParkArm = 0
return
;-------------Sub H2 Init
H2Init
serout SSC32,i38400,[13] ;clear the SSC-32 buffers
;Servo Offset Command
;Replace the section between the quotes with the values as described in the tutorial.
serout SSC32,i38400,"#0PO-100 #1PO55 #2PO-100 #3PO80 #4PO-100 #5PO-10 #16PO0 #17PO0 #18PO-100 #19PO0 #20PO-100 #21PO-85",13]
serout SSC32,i38400,"XS0 XSTOP",13] ;Stop the sequencer if running
serout SSC32,i38400,"LF1700 RF1300 LR1300 RR1700",13] ;Horizontal
serout SSC32,i38400,"LH",DEC 3000 - H2_RH," LM",DEC 3000 - H2_RM," LL",DEC 3000 - H2_RL,13] ;Vertical Left
serout SSC32,i38400,"RH",DEC H2_RH," RM",DEC H2_RM," RL",DEC H2_RL,13] ;Vertical Right
serout SSC32,i38400,"VS2500 HT500",13] ;Vertical Speed and Horizontal Time
serout SSC32,i38400,"XS0",13] ;Set Global Speed to 0
H2_High = H2_RH
H2_Middle = H2_RM
H2_Low = H2_RL
MiddleMode = 1
return
;-------------Sub Arm SetAngle then GetPos
SetAngle
TmpDistance = Distance * 127 - Hand_Length * COS(Wrist_TableAngle)
if YPos > 0 then
TmpYPos = YPos * 127 + Hand_Length * SIN(Wrist_TableAngle)
else
TmpYPos = Hand_Length * SIN(Wrist_TableAngle) - (-YPos) * 127 ; **** due to bug in Basic ****
endif
if TmpDistance < 0 then
TmpDistance = 0
if ArmDirection then
Distance = Distance - (XCoord / AxisSpeed)
else
Distance = Distance + (XCoord / AxisSpeed)
endif
endif
if TmpYPos > 0 then
TmpSEWSEW = (TmpYPos * TmpYPos + TmpDistance * TmpDistance) / 16129
else
TmpSEWSEW = ((-TmpYPos) * (-TmpYPos) + TmpDistance * TmpDistance) / 16129 ; **** due to bug in Basic ****
endif
TmpSEW = SQR(TmpSEWSEW)
if TmpSEW > (Arm_Length + Forearm_Length) then
TmpSEW = Arm_Length + Forearm_Length
if ArmDirection then
Distance = Distance - (XCoord / AxisSpeed)
else
Distance = Distance + (XCoord / AxisSpeed)
endif
YPos = YPos + (YCoord / AxisSpeed)
TmpSEWSEW = TmpSEW * TmpSEW
endif
TmpCos = -(Arm_Length * Arm_Length + Forearm_Length * Forearm_Length - TmpSEWSEW)
if TmpCos > 0 then
TmpCos = TmpCos * 127 / (2 * Arm_Length * Forearm_Length)
else
TmpCos = -((-TmpCos) * 127 / (2 * Arm_Length * Forearm_Length)) ; **** due to bug in Basic ****
endif
gosub ACos
If TmpAngle > Elbow_AngleMax then
Elbow_Angle = Elbow_AngleMax
elseif TmpAngle < Elbow_AngleMin
Elbow_Angle = Elbow_AngleMin
else
Elbow_Angle = TmpAngle
endif
TmpCos = TmpDistance / TmpSew
gosub ACos
Shoulder_Angle = TmpAngle
TmpCos = (Forearm_Length * Forearm_Length - Arm_Length * Arm_Length + TmpSEWSEW)
if TmpCos > 0 then
TmpCos = TmpCos * 127 / (2 * Forearm_Length * TmpSEW)
else
TmpCos = -((-TmpCos) * 127 / (2 * Forearm_Length * TmpSEW)) ; **** due to bug in Basic ****
endif
gosub ACos
if TmpYpos > 0 then
TmpAngle = TmpAngle + Shoulder_Angle
else
TmpAngle = TmpAngle - Shoulder_Angle
endif
If TmpAngle > Shoulder_AngleMax then
Shoulder_Angle = Shoulder_AngleMax
elseif TmpAngle < Shoulder_AngleMin
Shoulder_Angle = Shoulder_AngleMin
else
Shoulder_Angle = TmpAngle
endif
Wrist_Angle = Elbow_Angle - Shoulder_Angle - Wrist_AngleShift - Wrist_TableAngle
If Wrist_Angle > Wrist_AngleMax then
Wrist_Angle = Wrist_AngleMax
elseif Wrist_Angle < Wrist_AngleMin
Wrist_Angle = Wrist_AngleMin
endif
;-------------Sub Arm GetPos
GetPos
TmpDistance = Arm_Length * COS(Shoulder_Angle) + Forearm_Length * COS(Shoulder_Angle - Elbow_Angle) |
+ Hand_Length * COS(Shoulder_Angle - Elbow_Angle + Wrist_AngleShift + Wrist_Angle)
TmpYPos = Arm_Length * SIN(Shoulder_Angle) + Forearm_Length * SIN(Shoulder_Angle - Elbow_Angle) |
+ Hand_Length * SIN(Shoulder_Angle - Elbow_Angle + Wrist_AngleShift + Wrist_Angle)
return
;-------------Sub Arc Cosinus
ACos
if TmpCos > 0 then
TmpAngle = ArcCos(TmpCos)
else
TmpAngle = 128 - ArcCos(-TmpCos)
endif
return
;--------------------------------------------------------------------
The controller is a lynxmotion wireless, and thanks again for all the help
The program as you listed it is set up for the L5 arm. Is that what you have connected? If not that would explain the skewed behaviour.
Do you have the gripper connected to pin 26? You need to verify the rest of the servo connections as well.
I’m not sure what you are saying concerning the obstacle sensor.