i know i know, it been asked before but please excuse me.
I have added every bit of detail i can so you can understand the problem better. sorry if it gets confusing!
ok…
power good, new batterys, correct jumpers installed, used to work but now i have problems.
i have been running the same code for a while now and making small changes here and there but nothing crazy.
for the last week or so when i power up the legs kinda spread out to the sides, move around slowly in unison asif doing some weired seq out of the blue. *
just picking this out the hat…
…could the wireless receiver be picking up a rouge signal from somewhere?*
anyway i found out that my SSC indicater light has been flashing while this has been happening…
ABB 9v VL no jumpers,
SSC 9v VL jumpers installed on VS2=VS1,
and 6v on VS1 for servos,
38.4 baud.
FTDI USB-to-serial adapter and DB9 cable both from LM! been using new fresh bats too.
my SSC indicater is flashing fast so its grabbing signal from somewhere?
i have an LED i connected to the SSC though the VL to use the power from the 9v but iv noticed that when i plug in the 9V to the ABB the light (I added) on the SSC lights up and flashs using the power from the ABB. the only thing connecting the SSC to the ABB is the serial cable. like this but with two separate 9v’s:
i had taken the LED (one i added) off but problem is still here.
Also when i just connect the 9V to the SSCbefore appling power the ABB the ABB beeps two beeps per second???
Keep in mind this setup worked before and iv even had problems uploading code to the** ABB**. wouldnt over write. and i found out that is could have been a power issue but also could this be related to the problem stated above?
hope you can help? its either really obvious or something has gone really wrong!
Is the “main loop” getting told that there is a move to accomplish?
Is there a “canned” routine that is getting called?
If there are more then one “serial out” routine/instruction to send data to the SSC32, then comment them out, one by one to determine the source of the “commands”. Gotta be coming from somewhere!
Power issues and power up sequence may create serial port jibberish. I just tried some power up combinations and lxnxterm started receiving strings of jibberish which I guess originate from the ssc-32.
yes, BB2 is drawing a small amount of current though the serial connection making the Atom think its power source is low, this could be coursing the beep. beeping stops when BB2’s power is connected too
no, any code i try to upload is having the same problem, (Legs spread out, move around slowly) even my unmodified code. SSC blinking very fast.
“canned” like a "goto"command? no.
no effect. as mentioned anything i upload or do had no effect to the robot. it like the program doesnt want to stop, its alive.
;-----------------------------[SERIAL CONNECTIONS]
SSC_OUTPUT con P11 ;Output pin for (SSC32 RX) on BotBoard (Yellow)
SSC_INPUT con P10 ;Input pin for (SSC32 TX) on BotBoard (Red)
SSC_BAUD con i38400 ;SSC32 Data Modulation Rate
;--------------------------------------------------------------------
;-----------------------------[PS2 DualShock Controller]
PS2DAT con P12 ;PS2 Controller DAT (Brown)
PS2CMD con P13 ;PS2 controller CMD (Orange)
PS2SEL con P14 ;PS2 Controller SEL (Blue)
PS2CLK con P15 ;PS2 Controller CLK (White)
PadMode con $79
ps2Tolerance con 10 ;The deadzone for the analog input from the remote
POSITIVE con 1 ;if the solution is NEARLY possible
NEGITIVE con 0 ;if the solution is NOT possible
BUTTON_RELEASE con 1
BUTTON_HOLD con 0
;--------------------------------------------------------------------
;-----------------------------[SSC PIN NUMBERS]
RRCoxaPin con P0 ;Rear Right leg Hip Horizontal
RRFemurPin con P1 ;Rear Right leg Hip Vertical
RRTibiaPin con P2 ;Rear Right leg Knee
RFCoxaPin con P8 ;Front Right leg Hip Horizontal
RFFemurPin con P9 ;Front Right leg Hip Vertical
RFTibiaPin con P10 ;Front Right leg Knee
LRCoxaPin con P16 ;Rear Left leg Hip Horizontal
LRFemurPin con P17 ;Rear Left leg Hip Vertical
LRTibiaPin con P18 ;Rear Left leg Knee
LFCoxaPin con P24 ;Front Left leg Hip Horizontal
LFFemurPin con P25 ;Front Left leg Hip Vertical
LFTibiaPin con P26 ;Front Left leg Knee
HeadPin con P19 ;Head rotation
MircoPin con P30 ;Micro servo test
;--------------------------------------------------------------------
;-----------------------------[MIN/MAX ANGLES in degrees]
;Take the servo Pulse, subtract 1500, divide by 750, and mutply by 90
;(180 degree ranged servos)
;e.g 2400-1500/750*90=108
RRCoxa_MIN con -40 ;Mechanical limits of the RIGHT REAR Leg
RRCoxa_MAX con 40
RRFemur_MIN con -40
RRFemur_MAX con 50
RRTibia_MIN con -45
RRTibia_MAX con 45
;-------------------
RFCoxa_MIN con -40 ;Mechanical limits of the RIGHT FRONT Leg
RFCoxa_MAX con 40
RFFemur_MIN con -40
RFFemur_MAX con 50
RFTibia_MIN con -45
RFTibia_MAX con 45
;-------------------
LRCoxa_MIN con -40 ;Mechanical limits of the LEFT REAR Leg
LRCoxa_MAX con 40
LRFemur_MIN con -40
LRFemur_MAX con 50
LRTibia_MIN con -45
LRTibia_MAX con 45
;-------------------
LFCoxa_MIN con -40 ;Mechanical limits of the LEFT FRONT Leg
LFCoxa_MAX con 40
LFFemur_MIN con -40
LFFemur_MAX con 50
LFTibia_MIN con -45
LFTibia_MAX con 45
;-------------------
HeadPin_Min con -90 ;Mechanical limits of the BODY ROTATE
HeadPin_Max con 90
MircoPin_Min con -50 ;Mechanical limits of the MICRO SERVO
MircoPin_Max con 50
;--------------------------------------------------------------------
;-----------------------------[BODY/LEG DIMENSIONS]
CoxaLength con 55 ;Length of the Coxa [mm]
FemurLength con 96 ;Length of the Femur [mm]
TibiaLength con 145 ;Lenght of the Tibia [mm]
CoxaAngle con 60 ;Default Coxa setup angle
RFOffsetX con -65 ;Distance X from center of the body to the Right Front coxa
RFOffsetZ con -65 ;Distance Z from center of the body to the Right Front coxa
RROffsetX con -65 ;Distance X from center of the body to the Right Rear coxa
RROffsetZ con 65 ;Distance Z from center of the body to the Right Rear coxa
LFOffsetX con 65 ;Distance X from center of the body to the Left Front coxa
LFOffsetZ con -65 ;Distance Z from center of the body to the Left Front coxa
LROffsetX con 65 ;Distance X from center of the body to the Left Rear coxa
LROffsetZ con 65 ;Distance Z from center of the body to the Left Rear coxa
;-----------------------------[ANGLES]
RFCoxaAngle var sword ;Actual Angle of the Right Front Leg
RFFemurAngle var sword
RFTibiaAngle var sword
RRCoxaAngle var sword ;Actual Angle of the Right Rear Leg
RRFemurAngle var sword
RRTibiaAngle var sword
LFCoxaAngle var sword ;Actual Angle of the Left Front Leg
LFFemurAngle var sword
LFTibiaAngle var sword
LRCoxaAngle var sword ;Actual Angle of the Left Rear Leg
LRFemurAngle var sword
LRTibiaAngle var sword
HeadAngle var sword ;Actual Angle of the Body rotate
;--------------------------------------------------------------------
;-----------------------------[POSITIONS]
RFPosX var sword ;Actual Position of the Right Front Leg
RFPosY var sword
RFPosZ var sword
RRPosX var sword ;Actual Position of the Right Rear Leg
RRPosY var sword
RRPosZ var sword
LFPosX var sword ;Actual Position of the Left Front Leg
LFPosY var sword
LFPosZ var sword
LRPosX var sword ;Actual Position of the Left Rear Leg
LRPosY var sword
LRPosZ var sword
;--------------------------------------------------------------------
;-----------------------------[INPUTS]
butA var bit
butB var bit
butC var bit
prev_butA var bit
prev_butB var bit
prev_butC var bit
;--------------------------------------------------------------------
;-----------------------------[OUTPUTS]
LedA var bit ;Red
LedB var bit ;Green
LedC var bit ;Orange
;--------------------------------------------------------------------
Index var byte ;Index used for freeing the servos
SSCDone var byte ;To check if SSC cycle is done
;GetSinCos
AngleDeg var float ;Input Angle in degrees
ABSAngleDeg var float ;Absolute value of the Angle in Degrees
AngleRad var float ;Angle in Radian
sinA var float ;Output Sinus of the given Angle
cosA var float ;Output Cosinus of the given Angle
;GetBoogTan
BoogTanX var sword ;Input X
BoogTanY var sword ;Input Y
BoogTan var float ;Output BOOGTAN2(X/Y)
;Body position
BodyPosX var sword ;Global Input for the position of the body
BodyPosY var sword
BodyPosZ var sword
;-----------------------------[Body Inverse Kinematics]
BodyRotX var sbyte ;Global Input pitch of the body
BodyRotY var sbyte ;Global Input rotation of the body
BodyRotZ var sbyte ;Global Input roll of the body
PosX var sword ;Input position of the feet X
PosZ var sword ;Input position of the feet Z
PosY var sword ;Input position of the feet Y
RotationY var sbyte ;Input for rotation of a single foot for the gait
BodyOffsetX var sbyte ;Input Offset betweeen the body and Coxa X
BodyOffsetZ var sbyte ;Input Offset betweeen the body and Coxa Z
sinB var float ;Sin buffer for BodyRotX calculations
cosB var float ;Cos buffer for BodyRotX calculations
sinG var float ;Sin buffer for BodyRotZ calculations
cosG var float ;Cos buffer for BodyRotZ calculations
TotalX var sword ;Total X distance between the center of the body and the feet
TotalZ var sword ;Total Z distance between the center of the body and the feet
BodyIKPosX var sword ;Output Position X of feet with Rotation
BodyIKPosY var sword ;Output Position Y of feet with Rotation
BodyIKPosZ var sword ;Output Position Z of feet with Rotation
;-----------------------------[Leg Inverse Kinematics]
IKFeetPosX var sword ;Input position of the Feet X
IKFeetPosY var sword ;Input position of the Feet Y
IKFeetPosZ var sword ;Input Position of the Feet Z
IKFeetPosXZ var sword ;Length between the coxa and feet
IKCF var float ;Length between coxa and femur
IKA1 var float ;Angle between coxa and femur line and the ground in rad
IKA2 var float ;Angle between femur and tibia line and the ground in rad
IKSolution var bit ;Output POSITIVE if the solution is possible
IKSolutionWarning var bit ;Output POSITIVE if the solution is NEARLY possible
IKSolutionError var bit ;Output POSITIVE if the solution is NOT possible
IKFemurAngle var sword ;Output Angle of Femur in degrees
IKTibiaAngle var sword ;Output Angle of Tibia in degrees
IKCoxaAngle var sword ;Output Angle of Coxa in degrees
;--------------------------------------------------------------------
;-----------------------------[PS2 DualShock Controller]
DualShock var Byte(7)
LastButton var Byte(2)
PS2Mode var Byte
BodyYShift var sbyte
;--------------------------------------------------------------------
;-----------------------------[TIMING]
lTimerWOverflowCnt var long ;Used in WTimer overflow. Will keep a 16 bit overflow so we have a 32 bit timer
lCurrentTime var long ;Simple function to get the current time
lTimerStart var long ;Start time of the calculation cycles
lTimerEnd var long ;End time of the calculation cycles
CycleTime var byte ;Total Cycle time
SSCTime var word ;This changes the servos speed directly. 500 = 0.5sec
PrevSSCTime var word ;Previous time for the changes the servos speed.
InputTimeDelay var byte ;Delay on the input
;--------------------------------------------------------------------
;-----------------------------[GLOBAL]
QuadOn var bit ;Power up Stalker (Standby)
;--------------------------------------------------------------------
;-----------------------------[BALANCE]
BalanceMode var bit ;Balance mode
TotalTransX var sword ;Total X translation of the body
TotalTransZ var sword ;Total Z translation of the body
TotalTransY var sword ;Total Y translation of the body
TotalYbal var sword ;Total Y Balance of the body
TotalXBal var sword ;Total X Balance of the body
TotalZBal var sword ;Total Z Balance of the body
TotalY var sword ;Total Y distance between the center of the body and the feet
;-----------------------------[GAIT]
GaitType var byte ;Gait type
TitularGSpeed var byte ;Supposed speed of the gait
LegLiftHeight var byte ;Current Travel height
TravelLengthX var sword ;Current Travel length X (Dualshock(5) Left joystick (Left - Right)
TravelLengthZ var sword ;Current Travel length Z (Dualshock(6) Left joystick (Up - Down)
TravelRotationY var sword ;Current Travel Rotation Y (Dualshock(3) Right joystick (Left - Right)
TravelLengthXZ var sword ;Current Travel Rotation X/Z (Dualshock(5/6) Left joystick (Left-Right/Up-down)
TLDivFactor var byte ;Number of steps that a leg is on the floor while walking
NrLiftedPos var nib ;Number of positions that a single leg is lifted (1-3)
HalfLiftHeigth var bit ;If POSITIVE the outer positions of the flighted legs will be half height
GaitInMotion var bit ;Temp to check if the gait is in motion
StepsInGait var byte ;Number of steps in gait
LastLeg var bit ;POSITIVE when the current leg is the last leg of the sequence
GaitStep var byte ;Actual Gait step
RFGaitLegNr var byte ;Init position of the leg
RRGaitLegNr var byte
LFGaitLegNr var byte
LRGaitLegNr var byte
GaitLegNr var byte ;Input Number of the leg
;---------
RFGaitPosX var sbyte ;Relative position corresponding to the Gait
RFGaitPosY var sbyte
RFGaitPosZ var sbyte
RFGaitRotY var sbyte ;Relative rotation corresponding to the Gait
;---------
RRGaitPosX var sbyte ;Relative position corresponding to the Gait
RRGaitPosY var sbyte
RRGaitPosZ var sbyte
RRGaitRotY var sbyte ;Relative rotation corresponding to the Gait
;---------
LFGaitPosX var sbyte ;Relative position corresponding to the Gait
LFGaitPosY var sbyte
LFGaitPosZ var sbyte
LFGaitRotY var sbyte ;Relative rotation corresponding to the Gait
;---------
LRGaitPosX var sbyte ;Relative position corresponding to the Gait
LRGaitPosY var sbyte
LRGaitPosZ var sbyte
LRGaitRotY var sbyte ;Relative rotation corresponding to the Gait
;---------
GaitPosX var sbyte ;In-/Output Pos X of feet
GaitPosY var sword ;In-/Output Pos Y of feet
GaitPosZ var sbyte ;In-/Output Pos Z of feet
GaitRotY var sbyte ;In-/Output Rotation Y of feet
;====================================================================
;-----------------------------[TIMER INTERRUPT INIT]
ONASMINTERRUPT TIMERWINT, Handle_TIMERW
;-----------------------------[INITIALISATION]
;Turning off all the leds
LedA = 0
LedB = 0
LedC = 0
;The robot’s right front leg is (-,-) on the axis and it’s rear left leg is (+,+).
;Z-axis is front to back.
;X-axis is side to side.
;Y-axis is down (+) and up (-)
;Feet Positions in mm
RFPosX = 108 ;Start positions of the Right Front leg
RFPosY = 25
RFPosZ = -108
RRPosX = 108 ;Start positions of the Right Rear leg
RRPosY = 25
RRPosZ = 108
LFPosX = 108 ;Start positions of the Left Front leg
LFPosY = 25
LFPosZ = -108
LRPosX = 108 ;Start positions of the Left Rear leg
LRPosY = 25
LRPosZ = 108
;Right Front leg
GOSUB BodyIK -RFPosX+BodyPosX+RFGaitPosX, RFPosZ+BodyPosZ+RFGaitPosZ,RFPosY+BodyPosY+BodyYShift+RFGaitPosY, RFOffsetX, RFOffsetZ, RFGaitRotY]
GOSUB LegIK [RFPosX-BodyPosX+BodyIKPosX-RFGaitPosX, RFPosY+BodyPosY+BodyYShift-BodyIKPosY+RFGaitPosY, RFPosZ+BodyPosZ-BodyIKPosZ+RFGaitPosZ]
RFCoxaAngle = IKCoxaAngle + CoxaAngle ;Angle for the basic setup for the front leg right
RFFemurAngle = IKFemurAngle
RFTibiaAngle = IKTibiaAngle
;Right Rear leg
GOSUB BodyIK -RRPosX+BodyPosX+RRGaitPosX, RRPosZ+BodyPosZ+RRGaitPosZ,RRPosY+BodyPosY+BodyYShift+RRGaitPosY, RROffsetX, RROffsetZ, RRGaitRotY]
GOSUB LegIK [RRPosX-BodyPosX+BodyIKPosX-RRGaitPosX, RRPosY+BodyPosY+BodyYShift-BodyIKPosY+RRGaitPosY, RRPosZ+BodyPosZ-BodyIKPosZ+RRGaitPosZ]
RRCoxaAngle = IKCoxaAngle - CoxaAngle ;Angle for the basic setup for the Rear leg right
RRFemurAngle = IKFemurAngle
RRTibiaAngle = IKTibiaAngle
;Left Front leg
GOSUB BodyIK [LFPosX-BodyPosX+LFGaitPosX, LFPosZ+BodyPosZ+LFGaitPosZ,LFPosY+BodyPosY+BodyYShift+LFGaitPosY, LFOffsetX, LFOffsetZ, LFGaitRotY]
GOSUB LegIK [LFPosX+BodyPosX-BodyIKPosX+LFGaitPosX, LFPosY+BodyPosY+BodyYShift-BodyIKPosY+LFGaitPosY, LFPosZ+BodyPosZ-BodyIKPosZ+LFGaitPosZ]
LFCoxaAngle = IKCoxaAngle + CoxaAngle ;Angle for the basic setup for the front leg left
LFFemurAngle = IKFemurAngle
LFTibiaAngle = IKTibiaAngle
;Left Rear leg
GOSUB BodyIK [LRPosX-BodyPosX+LRGaitPosX, LRPosZ+BodyPosZ+LRGaitPosZ,LRPosY+BodyPosY+BodyYShift+LRGaitPosY, LROffsetX, LROffsetZ, LRGaitRotY]
GOSUB LegIK [LRPosX+BodyPosX-BodyIKPosX+LRGaitPosX, LRPosY+BodyPosY+BodyYShift-BodyIKPosY+LRGaitPosY, LRPosZ+BodyPosZ-BodyIKPosZ+LRGaitPosZ]
LRCoxaAngle = IKCoxaAngle - CoxaAngle ;Angle for the basic setup for the Rear leg left
LRFemurAngle = IKFemurAngle
LRTibiaAngle = IKTibiaAngle
GOSUB HeadTracking
GOSUB CheckAngles
LedC = IKSolutionWarning
LedA = IKSolutionError
;Read input
GOSUB Ps2Input
;Get endtime and calculate wait time
GOSUB GetCurrentTime], lTimerEnd
CycleTime = (lTimerEnd-lTimerStart)/WTIMERTICSPERMS
IF(QuadOn)THEN
;Wait for previous commands to be completed while walking
IF(ABS(TravelLengthX)>ps2Tolerance | ABS(TravelLengthZ)>ps2Tolerance | ABS(TravelRotationY*2)>ps2Tolerance) THEN
pause (PrevSSCTime - CycleTime -50) MIN 1 ;Min 1 ensures that there alway is a value in the pause command
IF(BalanceMode=0)THEN
SSCTime = TitularGSpeed + (InputTimeDelay*2)
ELSE
SSCTime = TitularGSpeed + (InputTimeDelay*2) + 100
ENDIF
ELSE
SSCTime = 200 ;TitularGSpeed 0,20 sec
ENDIF
GOSUB ServoDriver
;Add here (GOSUB ALL1500)
ELSE
;Turn on standby
GOSUB FreeServos
ENDIF
goto main
;IF ((ABS(TravelLengthX)>ps2Tolerance | ABS(TravelLengthZ)>ps2Tolerance) & TravelLengthZ<ps2Tolerance) | ABS(TravelRotationY*10)>ps2Tolerance THEN
IF ABS(TravelLengthX)>ps2Tolerance | ABS(TravelLengthZ)>ps2Tolerance THEN
;Calculate walking direction X and Z
TravelLengthXZ = SQR((TravelLengthX * TravelLengthX) + TravelLengthZ * TravelLengthZ)
HeadAngle = TOINT(FACOS(TOFLOAT(TravelLengthZ) / TOFLOAT(TravelLengthXZ)) * 180.0 / 3.141592)-180
;Add sign depending on the direction of X
HeadAngle = HeadAngle * (TravelLengthX/ABS(TravelLengthX))
ENDIF
;Calculate body angle depending on rotation
IF ABS(TravelRotationY2)>ps2Tolerance & ABS(TravelRotationY3) > ABS(HeadAngle) THEN
HeadAngle = -TravelRotationY3 ; Rotation max = 166 to get max range of 90 deg.
ENDIF
RETURN
;====================================================================
;[ReadButtons] Reading input buttons from the ABB
ReadButtons:
input P4
input P5
input P6
butA = IN4
butB = IN5
butC = IN6
return
;--------------------------------------------------------------------
;[WriteLEDs] Updates the state of the leds
WriteLEDs:
if ledA = 1 THEN
low p4
ENDIF
if ledB = 1 THEN
low p5
ENDIF
if ledC = 1 THEN
low p6
ENDIF
return
;-------------------------------------------------------------------------------------------------------------------
;-----------------------------[PS2 INPUT]
Ps2Input:
low PS2SEL
shiftout PS2CMD,PS2CLK,FASTLSBPRE,$1\8]
shiftin PS2DAT,PS2CLK,FASTLSBPOST,[PS2Mode\8]
high PS2SEL
pause 1
ELSE
'Turn on
Sound P9,[60\400,80\4500,100\5000]
SSCTime = 200 ;0,20 sec
QuadOn = POSITIVE
ENDIF
ENDIF
;------------------
IF QuadOn THEN
IF (DualShock(1).bit0 = 0) and LastButton(0).bit0 THEN ;Select Button test
IF TravelLengthX=0 & TravelLengthZ=0 & TravelRotationY=0 THEN
;Switch to next Gait type
IF GaitType<2 THEN
Sound P9,[50\4000]
GaitType = GaitType+1
ELSE
Sound P9,[50\4000, 50\4500]
GaitType = 0
ENDIF
GOSUB GaitSelect
ENDIF
ENDIF
ENDIF
;------------------
; IF (DualShock(1).bit4 = 0) and LastButton(0).bit4 THEN ;D-Up Button test
; ENDIF
;------------------
; IF (DualShock(1).bit6 = 0) and LastButton(0).bit6 THEN ;D-Down Button test
; ENDIF
;------------------
; IF (DualShock(1).bit5 = 0) and LastButton(0).bit5 THEN ;D-Right Button Test
; ENDIF
;------------------
; IF (DualShock(1).bit7 = 0) and LastButton(0).bit7 THEN ;D-Left Button Test
; ENDIF
;------------------
IF (DualShock(2).bit4 = 0) THEN ;Triangle Button test
BodyPosY = 35 ;crouch mode
SSCTime = 2000 ;2,00 sec
ENDIF
;------------------
IF QuadOn THEN
IF (DualShock(2).bit5 = 0) and LastButton(1).bit5 THEN ;Circle Button test
BodyPosY = 120 ;search mode
SSCTime = 2000 ;2,00 sec
ENDIF
;------------------
; IF (DualShock(2).bit6 = 0) and LastButton(1).bit6 THEN ;Cross Button test
; ENDIF
;------------------
IF (DualShock(2).bit7 = 0) and LastButton(1).bit7 THEN ;Square Button test
IF BalanceMode = 0 THEN
BalanceMode = 1
sound P9,[100\4000, 50\8000]
ELSE
BalanceMode = 0
sound P9,[250\3000]
ENDIF
ENDIF
;------------------
; IF (DualShock(2).bit3 = 0) and LastButton(1).bit3 THEN ;R1 Button test
;------------------
;Calculate walking time delay
InputTimeDelay = 128 - (ABS((Dualshock(5) - 128)) MIN ABS((Dualshock(6) - 128))) MIN ABS((Dualshock(3) - 128))
ENDIF
;------------------
LastButton(0) = DualShock(1)
LastButton(1) = DualShock(2)
return
;-------------------------------------------------------------------------------------------------------------------
;-----------------------------[GAIT TYPE Select]
GaitSelect
;Gait selector
;------------------
IF (GaitType = 2) THEN ;12 step wave gait for a quad
LRGaitLegNr = 8 ;Init position of the leg
RFGaitLegNr = 11
RRGaitLegNr = 2
LFGaitLegNr = 5
NrLiftedPos = 3 ;Number of positions that a single leg is lifted (1-3)
HalfLiftHeigth = NEGITIVE ;flighted legs will be half height
TLDivFactor = 10 ;Number of steps that a leg is on the floor while walking
StepsInGait = 12 ;Number of steps in gait
TitularGSpeed = 100 ;Supposed speed of the gait
ENDIF
return ;gait by Innerbreed
;------------------
IF (GaitType = 1) THEN ;12 step wave gait for a quad
LRGaitLegNr = 8
RFGaitLegNr = 11
RRGaitLegNr = 2
LFGaitLegNr = 5
;Check IF the Gait is in motion
GaitInMotion = ((ABS(TravelLengthX)>ps2Tolerance) | (ABS(TravelLengthZ)>ps2Tolerance) | (ABS(TravelRotationY)>ps2Tolerance) )
;Leg middle up position
;Gait in motion Gait NOT in motion, return to home position
IF (GaitInMotion & (NrLiftedPos=1 | NrLiftedPos=3) & GaitStep=GaitLegNr) | (GaitInMotion=NEGITIVE & GaitStep=GaitLegNr & ((ABS(GaitPosX)>2) | (ABS(GaitPosZ)>2) | (ABS(GaitRotY)>2))) THEN ;Up
GaitPosX = 0
GaitPosY = -LegLiftHeight
GaitPosZ = 0
GaitRotY = 0
ELSE
;Optional Half heigth Rear
IF ((NrLiftedPos=2 & GaitStep=GaitLegNr) | (NrLiftedPos=3 & (GaitStep=GaitLegNr-1 | GaitStep=GaitLegNr+(StepsInGait-1)))) & GaitInMotion THEN
GaitPosX = -TravelLengthX/2
GaitPosY = -LegLiftHeight/(HalfLiftHeigth+1)
GaitPosZ = -TravelLengthZ/2
GaitRotY = -TravelRotationY/2
ELSE
;Optional half heigth front
IF (NrLiftedPos>=2) & (GaitStep=GaitLegNr+1 | GaitStep=GaitLegNr-(StepsInGait-1)) & GaitInMotion THEN
GaitPosX = TravelLengthX/2
GaitPosY = -LegLiftHeight/(HalfLiftHeigth+1)
GaitPosZ = TravelLengthZ/2
GaitRotY = TravelRotationY/2
ELSE
;Leg front down position
IF (GaitStep=GaitLegNr+NrLiftedPos | GaitStep=GaitLegNr-(StepsInGait-NrLiftedPos)) & GaitPosY<0 THEN
GaitPosX = TravelLengthX/2
GaitPosY = 0
GaitPosZ = TravelLengthZ/2
GaitRotY = TravelRotationY/2
;Move body forward
ELSE
GaitPosX = GaitPosX - (TravelLengthX/TLDivFactor)
GaitPosY = 0
GaitPosZ = GaitPosZ - (TravelLengthZ/TLDivFactor)
GaitRotY = GaitRotY - (TravelRotationY/TLDivFactor)
ENDIF
ENDIF
ENDIF
ENDIF
;Advance to the next step
IF LastLeg THEN ;The last leg in this step
GaitStep = GaitStep+1
IF GaitStep>StepsInGait THEN
GaitStep = 1
ENDIF
ENDIF
return
;--------------------------------------------------------------------
;-----------------------------[BalCalcOneLeg]
BalCalcOneLeg [PosX, PosZ, PosY, BodyOffsetX, BodyOffsetZ]
;Calculating totals from center of the body to the feet
TotalZ = BodyOffsetZ+PosZ
TotalX = BodyOffsetX+PosX
TotalY = 150 + PosY’ using the value 150 to lower the centerpoint of rotation 'BodyPosY +
TotalTransY = TotalTransY + PosY
TotalTransZ = TotalTransZ + TotalZ
TotalTransX = TotalTransX + TotalX
gosub GetBoogTan [TotalX, TotalZ]
TotalYbal = TotalYbal + TOINT((BoogTan180.0) / 3.141592)
gosub GetBoogTan [TotalX, TotalY]
TotalZbal = TotalZbal + TOINT((BoogTan180.0) / 3.141592)
gosub GetBoogTan [TotalZ, TotalY]
TotalXbal = TotalXbal + TOINT((BoogTan*180.0) / 3.141592)
return
;--------------------------------------------------------------------
;-----------------------------[BalanceBody]
BalanceBody:
TotalTransZ = TotalTransZ/6
TotalTransX = TotalTransX/6
TotalTransY = TotalTransY/6
if TotalYbal < -180 then 'Tangens fix caused by +/- 180 deg
TotalYbal = TotalYbal + 360
endif
if TotalZbal < -180 then 'Tangens fix caused by +/- 180 deg
TotalZbal = TotalZbal + 360
endif
if TotalXbal < -180 then 'Tangens fix caused by +/- 180 deg
TotalXbal = TotalXbal + 360
endif
LFGaitPosY = LFGaitPosY - TotalTransY
LRGaitPosY = LRGaitPosY - TotalTransY
RFGaitPosY = RFGaitPosY - TotalTransY
RRGaitPosY = RRGaitPosY - TotalTransY
return
;--------------------------------------------------------------------
;-----------------------------[GETSINCOS]
;Get the sinus and cosinus from the angle +/- multiple circles
GetSinCos [AngleDeg]
;Get the absolute value of AngleDeg
IF AngleDeg < 0.0 THEN
ABSAngleDeg = AngleDeg *-1.0
ELSE
ABSAngleDeg = AngleDeg
ENDIF
;Shift rotation to a full circle of 360 deg -> AngleDeg // 360
IF AngleDeg < 0.0 THEN ;Negative values
AngleDeg = 360.0-(ABSAngleDeg-TOFLOAT(360*(TOINT(ABSAngleDeg/360.0))))
ELSE ;Positive values
AngleDeg = ABSAngleDeg-TOFLOAT(360*(TOINT(ABSAngleDeg/360.0)))
ENDIF
IF AngleDeg < 180.0 THEN ;Angle between 0 and 180
;Subtract 90 to shift range
AngleDeg = AngleDeg -90.0
;Convert degree to radials
AngleRad = (AngleDeg*3.141592)/180.0
SinA = FCOS(AngleRad) ;Sin o to 180 deg = cos(Angle Rad - 90deg)
CosA = -FSIN(AngleRad) ;Cos 0 to 180 deg = -sin(Angle Rad - 90deg)
ELSE ;Angle between 180 and 360
;Subtract 270 to shift range
AngleDeg = AngleDeg -270.0
;Convert degree to radials
AngleRad = (AngleDeg*3.141592)/180.0
SinA = -FCOS(AngleRad) ;Sin 180 to 360 deg = -cos(Angle Rad - 270deg)
CosA = FSIN(AngleRad) ;Cos 180 to 360 deg = sin(Angle Rad - 270deg)
ENDIF
return
;--------------------------------------------------------------------
;[BOOGTAN2] Gets the Inverse Tangus from X/Y with the where Y can be zero or negative
;BoogTanX - Input X
;BoogTanY - Input Y
;BoogTan - Output BOOGTAN2(X/Y)
GetBoogTan [BoogTanX, BoogTanY]
IF(BoogTanX = 0) THEN ; X=0 -> 0 or Pi
IF(BoogTanY >= 0) THEN
BoogTan = 0.0
ELSE
BoogTan = 3.141592
ENDIF
ELSE
TotalZ = BodyOffsetZ+PosZ ;Offset betweeen the body and Coxa Z & Input position of the feet Z
TotalX = BodyOffsetX+PosX ;Offset betweeen the body and Coxa X & Input position of the feet X
GOSUB GetSinCos [TOFLOAT(BodyRotX+TotalXBal)] ;Global Input Body pitch
SinG = SinA ;SinG buffer for BodyRotZ / SinA Output of AngleDeg
CosG = CosA ;CosG buffer for BodyRotZ / CosA Output of AngleDeg
GOSUB GetSinCos [TOFLOAT(BodyRotZ+TotalZBal)] ;Global Input Body roll
SinB = SinA ;SinB buffer for BodyRotX / SinA Output of AngleDeg
CosB = CosA ;CosB buffer for BodyRotX / CosA Output of AngleDeg
GOSUB GetSinCos [TOFLOAT(BodyRotY+RotationY+TotalYBal)]
return
;--------------------------------------------------------------------
;-----------------------------[LEG INVERSE KINEMATICS]
;Calculates the angles of the tibia and femur for the given position of the feet
LegIK [IKFeetPosX, IKFeetPosY, IKFeetPosZ]
IKFeetPosXZ = TOINT(FSQRT(TOFLOAT((IKFeetPosXIKFeetPosX)+(IKFeetPosZIKFeetPosZ)))) ;Input position of the Feet X/Y/Z
IF(IKCF < TOFLOAT(FemurLength+TibiaLength-30)) THEN
IKSolution = POSITIVE ;Output POSITIVE IF the solution is possible
ELSE
IF(IKCF < TOFLOAT(FemurLength+TibiaLength)) THEN
IKSolutionWarning = POSITIVE ;Output POSITIVE IF the solution is NEARLY possible
ELSE
IKSolutionError = POSITIVE ;Output POSITIVE IF the solution is NOT possible
ENDIF
ENDIF
return
;--------------------------------------------------------------------
;[CHECK ANGLES] Checks the mechanical limits of the servos
CheckAngles:
RFCoxaAngle = (RFCoxaAngle min RFCoxa_MIN) max RFCoxa_MAX
RFFemurAngle = (RFFemurAngle min RFFemur_MIN) max RFFemur_MAX
RFTibiaAngle = (RFTibiaAngle min RFTibia_MIN) max RFTibia_MAX
RRCoxaAngle = (RRCoxaAngle min RRCoxa_MIN) max RRCoxa_MAX
RRFemurAngle = (RRFemurAngle min RRFemur_MIN) max RRFemur_MAX
RRTibiaAngle = (RRTibiaAngle min RRTibia_MIN) max RRTibia_MAX
LFCoxaAngle = (LFCoxaAngle min LFCoxa_MIN) max LFCoxa_MAX
LFFemurAngle = (LFFemurAngle min LFFemur_MIN) max LFFemur_MAX
LFTibiaAngle = (LFTibiaAngle min LFTibia_MIN) max LFTibia_MAX
LRCoxaAngle = (LRCoxaAngle min LRCoxa_MIN) max LRCoxa_MAX
LRFemurAngle = (LRFemurAngle min LRFemur_MIN) max LRFemur_MAX
LRTibiaAngle = (LRTibiaAngle min LRTibia_MIN) max LRTibia_MAX
HeadAngle = (HeadAngle min HeadPin_MIN) max HeadPin_MAX
return
;--------------------------------------------------------------------
;[SERVO DRIVER] Updates the positions of the servos
ServoDriver:
;Front Right leg
serout SSC_OUTPUT,SSC_BAUD,"#",dec RFCoxaPin,“P”,dec TOINT(TOFLOAT(RFCoxaAngle+90)/0.10588238)+650]
serout SSC_OUTPUT,SSC_BAUD,"#",dec RFFemurPin,“P”,dec TOINT(TOFLOAT(RFFemurAngle+90)/0.10588238)+650]
serout SSC_OUTPUT,SSC_BAUD,"#",dec RFTibiaPin,“P”,dec TOINT(TOFLOAT(-RFTibiaAngle+90)/0.10588238)+650]
;Rear Right leg
serout SSC_OUTPUT,SSC_BAUD,"#",dec RRCoxaPin,“P”,dec TOINT(TOFLOAT(RRCoxaAngle+90)/0.10588238)+650]
serout SSC_OUTPUT,SSC_BAUD,"#",dec RRFemurPin,“P”,dec TOINT(TOFLOAT(RRFemurAngle+90)/0.10588238)+650]
serout SSC_OUTPUT,SSC_BAUD,"#",dec RRTibiaPin,“P”,dec TOINT(TOFLOAT(-RRTibiaAngle+90)/0.10588238)+650]
;Front Left leg
serout SSC_OUTPUT,SSC_BAUD,"#",dec LFCoxaPin,“P”,dec TOINT(TOFLOAT(-LFCoxaAngle+90)/0.10588238)+650]
serout SSC_OUTPUT,SSC_BAUD,"#",dec LFFemurPin,“P”,dec TOINT(TOFLOAT(-LFFemurAngle+90)/0.10588238)+650]
serout SSC_OUTPUT,SSC_BAUD,"#",dec LFTibiaPin,“P”,dec TOINT(TOFLOAT(LFTibiaAngle+90)/0.10588238)+650]
;Rear Left leg
serout SSC_OUTPUT,SSC_BAUD,"#",dec LRCoxaPin,“P”,dec TOINT(TOFLOAT(-LRCoxaAngle+90)/0.10588238)+650]
serout SSC_OUTPUT,SSC_BAUD,"#",dec LRFemurPin,“P”,dec TOINT(TOFLOAT(-LRFemurAngle+90)/0.10588238)+650]
serout SSC_OUTPUT,SSC_BAUD,"#",dec LRTibiaPin,“P”,dec TOINT(TOFLOAT(LRTibiaAngle+90)/0.10588238)+650]
PrevSSCTime = SSCTime
return
;--------------------------------------------------------------------
;[FREE SERVOS] Power-off all the servos
FreeServos
for Index = 0 to 31
serout SSC_OUTPUT,SSC_BAUD,"#",DEC Index,“P0”]
next
serout SSC_OUTPUT,SSC_BAUD,“T200”,13]
return
;-----------------------------------------------------------------------------------
;REGISTER Support
;(Handle TimerW interrupt)
BEGINASMSUB
HANDLE_TIMERW
push.w r1 ; save away register we will use
bclr #7,@TSRW:8 ; clear the overflow bit in the Timer status word
mov.w @LTIMERWOVERFLOWCNT+1:16,r1 ; We will increment the word that is the highword for a clock timer
inc.w #1,r1
mov.w r1, @LTIMERWOVERFLOWCNT+1:16
pop.w r1 ; restore our registers
rte ; and return
return
;-------------------------------------------------------------------------------------
;[Simple function to get the current time and verify that no overflow happened]
GetCurrentTime
lCurrentTime = lTimerWoverflowCnt + TCNT ; calculate the timer
IF lCurrentTime.highword <> lTimerWOverflowcnt.highword THEN
lCurrentTime = lTimerWoverflowCnt + TCNT ; calculate the timer
ENDIF
return lCurrentTIme
;--------------------------------------------------------------------END-----------------------------------[/code]
I think that the others covered the majority of the issues, but I will try a few somewhat random ones to see if they may help.
You say that you get a sound every about 2 seconds, this sounds like the PS2 init code… But like I have said in other threads, when I get things like this I like to put some distinct sound at the start of the program to see if it is resetting.
It sounds like you are gettig electrical connection between the two boards. I would probably remove the pin from the output of the SSC back to the BB2 (SSC_INPUT), since you are not using the Q command or the like.
Other than that I agree with Zenta to try to measure the different voltages when it is running. If you find that it is resetting and sometimes for other reasons, I like to take a jumper wire from lets say IO pins 4-6 on the BB2 and connect it up to the three pins connected to LEDS ABC. Then I start to turn on or off LEDS when I enter certain code. That makes it so that you can see if certain code is executing.
Ref 1/2:
it only sounds when i have direct power connected to the SSC not the BB2.
removed the pin but light still flashs.
i dont have the gear to test voltage but iv got lots of NEW 9v’s and power is good! i promis.
the other problem i explained is the moving of the legs, now i know its not anything i programmed as it now doesnt matter what i program into the atom im getting the Flashing LED on the SSC! even Xans original v1.3!
it stops when i take out the bodyik and legik code.
Sorry to hear the problem has returned. It sounds sort-of like it might be a loose wire/pin or an Intermindent short or something of that nature. When I run into issues like this I tend to use a sledge hammer approach to fix (no not a real one )
I would redo my power wires going into both the SSC-32 and BB2 and make extra care to make sure there are no parts of the wire exposed between the two for possible shorts. I would remove all except maybe one or two servos. If it works ok with the small number of servos, add a few more and if still works, add a few more. If at one of these steps it stops working properly, either problem with servo or with load…
I would ground myself and then press on the ATmega to make sure all of the pins our properly seated in the socket. If I was the one who put in the chip I would also check to make sure none of the leads are bent over.
I would if possible switch 6v batteries as maybe the current one is not holding a good charge. For my hex I purchased one of the battery checker displays that Lyxmotion sells: lynxmotion.com/Product.aspx?productID=463&CategoryID=105. It was very interesting to see how much the voltages can jump around when the Hex is on. I believe you said that you don’t have any volt meter available to test the voltages?
One way would be to use the voltage divides on pins 16 and 17 for VS and VL. You could also connect VS to the VS port on the BB2 and then at startup and maybe in your loop you could test the voltages with ADIN commands. There are others who have more electrical experience here, but I think there is a 30k resister before the tap and a 10K after the tap but before the ground, so the voltage would be divided by 4. So a 6v input should give an ADIN af about ((6/4)/5)*1024 = 307. Just a thought.
“sledge hammer approach” im almost there my friend! lol
i will disconnect everything and start working towards the problem. i have already arranged the power supplies in different ways so i dont think this is the prob. but maybe a trapped or pulled servo wire has shorted or something.
thanks for the pointers, ill try them tonight.
I forgot one other thing to look for, which got me in the past, is the stray screw/nut, that you forgot you dropped inside the case and later find it is sitting on the board or under the board, which when it rolls around may short things out…
Kurt
P.S. - The real sledgehammer approach does give some instant gratifications, but can be expensive!
With a lot of building, tearing down and rebuilding is it possible the DB9 or other connectors have been stressed resulting in an intermittent connection? The last time I received a servo controller for testing it had this problem. Of course after I replaced it the guy tells be he was controlling a bot that was to drive up stairs and the thing cam crashing down with DB9 cable in tow many many times over.
Forgive me for missing this post, and for not knowing the details. The Atom is sending commands but the servos are acting screwy? Have you tried moving to a different I/O pin on the Bot Board? Have you tried changing to a different baud rate? Could help to know if it works at a different baud rate.
i configured the SSC for PC communication a few days ago and the resualts were good (well bad). the led works as normal.
forgiven…
i have tried changing the baud rate. im afraid im still getting the same resaults.
the only thing i still have to check is the ATOM itself! like i said "any program i write to it, is the same.
first i thought that the atom wasnt actually writing 'and that i was just turning it off, connecting it to pc, wait for the ide to load nothing, and then turn on again with the same “orginal” program, but after programming it with
end
i soon realised it was writing as the atom was clear. and the flashing stops.
im sure its not the code although these resualts say otherwise…!
tomorrow i will look into resoldering the wireless PS2 cable to see if there are any loose connections there.
Yes remember P9 on the ABB and BB2 are connected to the speaker. So if SSC_OUTPUT is P9 then
everytime your program tries to output something to the SSC-32 it will generate some noise. That is unless you removed the speaker jumper.
)
