L6 ps2 control using atom pro

[fish-5]

I was unable to replicate your problem, however I was also unable to get the code directly off the website to work with the Atom Pro. So I made a few changes, and it works on this end. Try this code out.

Keep in mind, that in an effort to normalize things, I changed the pins used to the same as RIOS. So, it is now 0, 1, 2, etc instead of 5, 6, 7. So you’ll have to switch your servos back.

[code];**************************************************
;**** Basic Atom with SSC-32 and PS2 DualShock ****
;********* or MadCatz wireless controller *********
;**************** L5-L6 program #1 ****************
;*********** Bot Board Buzzer support *************
;**************************************************
;*** Programmer: Laurent Gay, [email protected] ***
;**************************************************
;
; 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

;Arm
BasePin con 0
Base_PulseDef con 1500
Base_PulseMin con 850
Base_PulseMax con 2150

ShoulderPin con 1
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 2
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 3
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 4
Grip_PulseDef con 1500
Grip_PulseMin con 1000
Grip_PulseMax con 1800

WristRotPin con 5
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)
ParkArm var Bit
NeedServosOn var Bit
ArmDirection var Bit ;1 = Left to right, 0 = right to left
ArmOnOff var Bit
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

LastButton(0) = 255
LastButton(1) = 255

;Arm engine
ArmDirection = 0 ;1 = Left to right, 0 = Right to left
ArmOnOff = 1
ParkArm = 0
NeedServosOn = 1
gosub ArmInit

pause 500

;--------------------------------------------------------------------
;-------------Main loop
main
;DS2
low SEL
shiftout CMD,CLK,FASTLSBPRE,$1\8,$42\8]
shiftin DAT,CLK,FASTLSBPOST,[DualShock(0)\8, DualShock(1)\8, DualShock(2)\8, DualShock(3)\8, |
DualShock(4)\8, DualShock(5)\8, DualShock(6)\8]
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

;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 or NeedServosOn 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
pause 36

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]	
	Time = 750
endif
ArmOnOff = 1
ParkArm = 0

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

;--------------------------------------------------------------------[/code]

[kurte]

The other thing to verify that it is not the Basic Atom Pro that is resetting. I normally add some type of distinctive sound at the start that if it repeats, I know something is wrong. Maybe something like:

sound Speaker, [100\880, 100\988, 100\1046, 100\1175] ;musical notes, A,B,C,D. 

I would put it maybe right after the ;------------- Init
comment.

If you don’t hit any of the buttons, but simply move the joysticks, does it move at all?

Kurt

[stamanol]

Thanks James! Now it works! I can move the arm with the joysticks. It was not a power issue after all or maybe it was but it was purely code dependent. However this code needs improvement. The home position of the arm is not correct(it regards its home position as being fully extended forward).Also I can close and open the grip while rotating the wrist.How can I do it independedly.Also after I press a button(dont remember which) the grip remains always closed and I cannot reopen it again no matter what I press. Furthermore the R1 button seems to have the same effect as the triangle button but according to the comments on the beggining this is not its purpose.

If Im not asking too much,will you improve the code? Or at least give me some basic guidelines of what values to play with within the code and what changes did you make that fixed it so that I can start experimenting with although im almost a total begginer and have not got the expertise to improve myself yet.
Well done James and thanks again!

[fish-5]

@stamanol

Thanks James! Now it works! I can move the arm with the joysticks.

Whew… there’s one headache out of the way. Glad it’s working now

@stamanol

The home position of the arm is not correct(it regards its home position as being fully extended forward).

Are you sure you have all the servos plugged into the proper pins?

@stamanol

Also I can close and open the grip while rotating the wrist.How can I do it independedly.

This also points to not having the servos plugged in correctly.

@stamanol

Also after I press a button(dont remember which) the grip remains always closed and I cannot reopen it again no matter what I press.

Can you move anything else, or can you just not move the gripper?

@stamanol

Furthermore the R1 button seems to have the same effect as the triangle button but according to the comments on the beggining this is not its purpose.

I’m not too sure about this one. Make sure you have everything plugged in correctly, then see if this still happens.

[robotdude]

@stamanol

The home position of the arm is not correct(it regards its home position as being fully extended forward).

Perhaps you could link an image of the arm in it’s home and all=1500 positions.

[stamanol]

All cables are in the correct pins. I was not able to reproduce again not be able to open the grip.
Every time the program begins it goes to the extended position.However I found out that when pressing the left joystick button downwards (documented as invert x axis) it gains the home position.I dont know if this is normal. Also movements with the right joystick are very violent.
Finally the R1 button is not functioning properly because it just disables the servos whereas it is supposed to turn the wrist.

Please calm me down because I was a witness of something awfull! I saw a small puff of smoke coming out of the botboard when the arm was making the violent movements, so I shut it down immediatelly.However when I reopened it everything seemed fine.Also I didnt smell anything. (this might help you but it may be possible a shortcircuit was made between the two boards,although I was pretty sure they were not touching each other,because I have the ssc-32 board loose and not fixed on top of the bot board-yeah I know im a jerk-but how else would I have access to the bot board below now that im continuously pluging and unplugging cables and making jumper configurations?)

[robotdude]

stamanol, please calm down.

We sort of got things way out of order here. Now that the program is known to work with the atom pro we need to go back to the beginning. Can you review the assembly guides to make sure everything was assembled correctly?

We are making a lot of assumptions here. We need to review the powering of the arm, the jumper settings, the type of game controller you are using. Please take a few minutes to let us know exactly what you have there. A few images would be great. We need input…

[stamanol]

I found a piece of hair in between the servo pins of the ssc-32…Could it be that also in the botboard(I didnt find anything unless it was vaporized)?
How can I check if the board is completely functional? Could it be working correctly but a subcircuit burned? Or there is no such case?

[robotdude]

No earthly idea sorry…

smoke = heat = discoloration = visible clue

[stamanol]

OK now im calmer and more organised. Im sorry but I dont have a camera now in handy,so I will try to explain with a drawing.Besides the cables are so messed up that you would have a hard time figuring out a schematic.
The first drawing is the powering configuration before you told me to power the ssc-32 logic with a 9V battery and the second drawing is with the 9V battery.

Please tell me if there is a way to check if the boards are internally ok. Everything works fine though.

[robotdude]

Gulp…

This is no help really. I can’t tell if the wires are big enough to handle the current required. I can’t see if you are observing proper wiring practices. I have a bad feeling about the wiring, and suspect it may be the cause of some of your problems. For example look at this diagram. This is an example of how the arm should be wired.

[stamanol]

Sorry

I thought you cared only about the connections.This is not the actual wiring but a representation of the connections made. The actual nodes where 3 cables meet are done in different places and are soldered with ordinary solder and are insulated with heat shrink tubes.The extra cables that I have put are only very slightly bit less fat than the ones on the wire harnesses by lynxmotion.Is this ok? What else should I be carefull with?