Hello all
I’m working with a Basic Atom 28D (the PIC16F886 chip), a Bot Board II, and the BasicMicro IDE 05.3.1.5 and I have run into a compiler error when I attempt to compile using the ADIN command.
I am working in the same lab (at college) as RougeSardaukar (he posted a few days ago & I can’t link to it since I’m too new) and followed the instructions that was given in response to his post about using the ADIN command. A simple example program that I wrote works fine with the board:
[code]feet var word(4)
Main
while 1
feet(0) = 0
feet(1) = 0
feet(2) = 0
feet(3) = 0
adin AX0,feet(0)
serout S_OUT,I8N1_9600,"foot0 = “, dec feet(0)]
adin AX1,feet(1)
serout S_OUT,I8N1_9600,” foot1 = “, dec feet(1)]
adin AX2,feet(2)
serout S_OUT,I8N1_9600,” foot2 = “, dec feet(2)]
adin AX3,feet(3)
serout S_OUT,I8N1_9600,” foot3 = ", dec feet(3) ,13]
wend
goto Main
end[/code]
Right now I’m trying to write a program for my hexapod robot using force sensing resistors on it’s feet. I have been adapting the “basic program” that PowerPod generates. It’s very messy but you should be able to see my problem.
The ADIN command that I’m trying to use is the first line in MAIN.
[code];This program is modified from the basic program that is created by the PowerPod program from Lynxmotion.
;This program was written by during the Summer of 2009
;This is an autonomous program for the circular hexapod with force sensing resistors (FSR) attached to each foot
;
;The robot will walk forward using a tripod gait until one of its feet no longer touchs the ground.
; Upon reaching this point it will back up several paces and turn in the direction opposite of the detected edge.
;Note: These motions assume that the robot is on a wide enough surface to back up.
;Note: The robot turns in place.
;
;Note: The back of the robot is considered to be where the servo power switch is located
;The legs are assigned numbers from 0 to 5. Leg 0 is the rear right leg. The rest are numbered going up
; in a counter clockwise fashion.
;
;The two tripods in the gait are labeled 0 and 1. Tripod 0 is legs 0, 2, & 4. Tripod 1 is legs 1, 3, & 5.
;
;Each FSR has been attached in this fashion:
;
; FSR 11 KOhms
; 5V—///—V_Out—///—Ground
;
;V_Out is attached to the respective digital input pin for each leg.
; (Leg 0’s V_Out goes to P0, Leg 1’s V_Out goes to P1, etc.)
;-------------Pin Declarations
input P0
input P1
input P2
input P3
input P4
input P5
input P12
;-------------Constants
;*** SSC-32 card communication on pin 8 ***
;This pin is used to communicate with the servo controller
SSC32 con p8
;Legs
;The constants for the legs:
;Minimum and maximum angles for the horizontal hips (60° from 90°)
HipH_AngleMin con 21 ;30°
HipH_AngleMax con 107 ;150°
;The respective pulse lengths that correspond to the max and min angles
HipH_PulseMin con 910
HipH_PulseMax con 2090
;Minimum and maximum angles for the vertical hips (55° from 90°)
HipV_AngleMin con 25 ;35°
HipV_AngleMax con 103 ;145°
;The respective pulse lengths that correspond to the max and min angles
HipV_PulseMin con 960
HipV_PulseMax con 2040
;Minimum and maximum angles for the knees (40° from 90°)
Knee_AngleMin con 36 ;50°
Knee_AngleMax con 107 ;150°
;The respective pulse lengths that correspond to the max and min angles
Knee_PulseMin con 1107
Knee_PulseMax con 2090
; 3DOF-A Leg Dimensions (TibiaAngle constant = 0)
;The leg demensions, modified include touch sensors
;The Tibia_Length is longer than what was given
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 143 ;5.625" = 143mm (5.625 * 25.4)
;How far out the knees will go when walking
KneeShiftPulse con 200 ;range is 0 to 255 (activated by pushing start button, affect the legs when up)
;When standing, the legs are straight up and down
TibiaAngle con 0 ;range is -20 (interior) to 20 (exterior), 0 is vertical, this is ‘all time’ tibia angle
;ACos
;Data writes data to the EEPROM memory space (“data {address} value” if no address then start at 0)
;This data is a look up table for the ArcCos subroutine
data 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
;All of the leg motors are defined like this
;First letter = F (Front), M (Middle), or R (Rear)
;Second Letter = R (Right) or L (Left)
;Third Letter = H (Hip) or K (Knee)
;Fourth Letter (if needed) = H (Horizontal) or V (Vertical)
;First Number (if needed) = second motor number (i.e. motor 5 is called 05)
;*******
;This system can be confusing so it might be better
;to just refer to the motors by their number
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”
;--------------------------------------------------------------------
;-------------Variables
;All variables are defined before hand in BASIC usually after the constants
;Used like an all purpose counter (like using “int x” in C++)
Index var Byte
;Control is used to provide instructions for the robot to move and turn
Control var Byte(3)
;
Steps var Byte
;Used as a flag to help switch between the two tripods
FlipFlap var Bit
;
MovesDelay var Sbyte
;Offsets for the vertical hip motors that specify the
;height of the body based off of the pitch and roll
HeightShift var Sbyte(6)
;The offsets for the knees, the last byte
;is able to save an offset for later use
KneeShift var Byte(7)
;Set by the control variable
Steering var Sbyte
;Set by the control variable
;Note: these are relative to the front of the hexapod
XSpeed var Sbyte
;Set by the control variable
;Note: these are relative to the front of the hexapod
YSpeed var Sbyte
;Helps decide the horizontal hip angles based on the steering
Rotate var Sbyte(6)
;Used to indicate the direction of motion
DCoord var Byte
;Corrisponding angle for the direction of motion
DAngle var Byte
;Holds the value of the GaitSpeed
NbSteps var Byte
;Indicates how fast the gait is
GaitSpeed var Byte
;Helper for changing the GaitSpeed
GaitSpeedTmp var Byte
;
StepFlag var Byte
;Holds a scaled version of a legs XPos; It is used in calculation
TmpXPos var Long
;Holds a scaled version of a legs YPos; It is used in calculation
TmpYPos var Long
;Holds a scaled version of a legs ZPos; It is used in calculation
TmpZPos var Long
;Holds a scaled version Distance; It is used in calculation
TmpDistance var Long
;Used in calculating the new positions of the legs
TmpSEW var Word
;Used in calculating the new positions of the legs
TmpSEWSEW var Long
;Used for the ACos subroutine (it’s the parameter of it)
TmpCos var Long
;Used for the ACos subroutine (it’s the return value of it)
TmpAngle var Sbyte
;X position (in cartsian coordinates) for each foot
XPos var Sword(6)
;Y position (in cartsian coordinates) for each foot
YPos var Sword(6)
;X position (in cartsian coordinates) for each foot
ZPos var Sword(6)
;Helps in the calculation of the new feet coordinates
XPos2 var Sword(6)
;Helps in the calculation of the new feet coordinates
YPos2 var Sword(6)
;Helps in the calculation of the new feet coordinates
ZPos2 var Sword(6)
;The distance that a foot will move in the next motion
Distance var Sword
;Used to hold the new angle for a horizontal hip when calculated
HipH_Angle var Sbyte
;Used to hold the new angle for a vertical hip when calculated
HipV_Angle var Sword
;Used to hold the new angle for a knee when calculated
Knee_Angle var Sword
;Used to hold the pulses for all the horizontal hips
HipH_Pulse var Word(6)
;Used to hold the pulses for all the vertical hips
HipV_Pulse var Word(6)
;Used to hold the pulses for all the knee
Knee_Pulse var Word(6)
;Used to indicate how high the legs should move up when walking
LegUpShift var Byte
;
Freeze var Bit
;Indicates which tripod is being moved
Tripod var Bit
;Flags to state which motion to use
MovingForward var Bit
TurningLeft var Bit
TurningRight var Bit
;Counters to allow for backing up and rotating
BackwardsCounter var Byte
TurningCounter var Byte
;Flags for determining stability
Tripod0 var Bit
Tripod1 var Bit
Leg0off var Bit
Leg1off var Bit
Leg2off var Bit
Leg3off var Bit
Leg4off var Bit
Leg5off var Bit
Temp var word
;--------------------------------------------------------------------
;***************
;*** Program ***
;***************
;-------------Init
;First the program initializes the robots state
;clear sets all user RAM (including all varialbles) to zero
clear
;A quick pause
pause 500
;-------------Init SSC-32 with pulse offsets
;These offsets were choosen by us through the PowerPod program
serout SSC32,I8N1_38400,"#", |
RRHH,RRHH2,“po-93 #”,RRHV,RRHV2,“po-53 #”,RRK,RRK2,“po21 #”, |
MRHH,MRHH2,“po97 #”,MRHV,MRHV2,“po55 #”,MRK,MRK2,“po69 #”, |
FRHH,FRHH2,“po69 #”,FRHV,FRHV2,“po44 #”,FRK,FRK2,“po21 #”, |
RLHH,RLHH2,“po-20 #”,RLHV,RLHV2,“po2 #”,RLK,RLK2,“po-45 #”, |
MLHH,MLHH2,“po-39 #”,MLHV,MLHV2,“po-11 #”,MLK,MLK2,“po-41 #”, |
FLHH,FLHH2,“po17 #”,FLHV,FLHV2,“po13 #”,FLK,FLK2,“po-71”,13]
;SSC-32 -> H3 engine
gosub H3Init
;Emitting two high frequency sounds
Sound 9,[100000\4235, 200000\4435]
LegUpShift = 35
GaitSpeed = 4
;--------------------------------------------------------------------
;-------------Main loop
main
adin AX0, Temp
; while 1
; if IN12 = 0 then
; goto here
; endif
; wend
; here
;Testing the foot sensors
;Tripod 0 is on the ground (1 is on, 0 is off)
if IN0 = 1 and IN2 = 1 and IN4 = 1 then
Tripod0 = 1
else
Tripod0 = 0
endif
;Tripod 1 is on the ground
if IN1 = 1 and IN3 = 1 and IN5 = 1 then
Tripod1 = 1
else
Tripod1 = 0
endif
;Check each leg to see if it's on or off the ground (0 is on, 1 is off)
if IN0 = 1 then
Leg0off = 0
else
Leg0off = 1
endif
if IN1 = 1 then
Leg1off = 0
else
Leg1off = 1
endif
if IN2 = 1 then
Leg2off = 0
else
Leg2off = 1
endif
if IN3 = 1 then
Leg3off = 0
else
Leg3off = 1
endif
if IN4 = 1 then
Leg4off = 0
else
Leg4off = 1
endif
if IN5 = 1 then
Leg5off = 0
else
Leg5off = 1
endif
;If either tripod is on the ground then set the move forward flag
;Note: At least one tripod is always on the ground while in the Tripod Gait
if Tripod0 = 1 or Tripod1 = 1 then
MovingForward = 1
else
MovingForward = 0
endif
;If Tripod 0 is not fully on the ground
if Tripod0 = 0 then
;Check to see if it's only one leg that is off the ground
;This would indicate that the robot has just reached the edge of the surface
if Leg0off = 1 and Leg2off = 0 and Leg4off = 0 or Leg0off = 0 and Leg2off = 1 and Leg4off = 0 or |
Leg0off = 0 and Leg2off = 0 and Leg4off = 1 then
;If that is true then set the backwards counter
BackwardsCounter = 30
;Also set the turning counter to let the robot turn away from the edge
TurningCounter = 30
endif
endif
;If Tripod 1 is not fully on the ground
if Tripod1 = 0 then
;Check to see if it's only one leg that is off the ground
;This would indicate that the robot has just reached the edge of the surface
if Leg1off = 1 and Leg3off = 0 and Leg5off = 0 or Leg1off = 0 and Leg3off = 1 and Leg5off = 0 or |
Leg1off = 0 and Leg3off = 0 and Leg5off = 1 then
;If that is true then set the backwards counter
BackwardsCounter = 30
;Also set the turning counter to let the robot turn away from the edge
TurningCounter = 30
endif
endif
;If the backwards counter was just set
if BackwardsCounter = 30 then
;Check to see if it was the right side whose leg was off the edge
if Leg0off = 1 or Leg1off = 1 or Leg2off = 1 then
;If it was then flag the robot to turn left
TurningLeft = 1
endif
;Check to see if it was the left side whose leg was off the edge
if Leg3off = 1 or Leg4off = 1 or Leg5off = 1 then
;If it was then flag the robot to turn right
TurningRight = 1
endif
endif
;When the robot is done turning reset the turning flags
if TurningCounter <= 0 then
TurningLeft = 0
TurningRight = 0
endif
;Note: for the control variables 128 is zero
;Control values for when one tripod is on the ground
if MovingForward = 1 and BackwardsCounter <= 0 and TurningCounter <= 0 then
Control(0) = 128
Control(1) = 100
Control(2) = 128
;Control values for when the supporting tripod is not on the ground
elseif BackwardsCounter > 0
Control(0) = 128
Control(1) = 156
Control(2) = 128
BackwardsCounter = BackwardsCounter - 1
;Control values for turning left
elseif TurningLeft = 1 and BackwardsCounter <= 0 and TurningCounter > 0
Control(0) = 128
Control(1) = 128
Control(2) = 100
TurningCounter = TurningCounter - 1
elseif TurningRight = 1 and BackwardsCounter <= 0 and TurningCounter > 0
Control(0) = 128
Control(1) = 128
Control(2) = 100
TurningCounter = TurningCounter - 1
else
;Control values for no movement
Control(0) = 128
Control(1) = 128
Control(2) = 128
endif
;
GaitSpeedTmp = GaitSpeed max 20
;This ensures that if the robot is walking too high then the speed will be limited;
; it can go faster if it's legs aren't going extremely high
GaitSpeed = GaitSpeedTmp min 3
;YSpeed is set based off of the first control variable
YSpeed = Control(0) - 128
;XSpeed is set based off of the second control variable
XSpeed = Control(1) - 128
;Steering is set based off of the third control variable
Steering = Control(2) - 128
;The StepFlag is set to represent the speed or if that is too large then 3
StepFlag = (GaitSpeedTmp - 2) max 3
;If Steering, YSpeed, or XSpeed is nonzero
if Steering or YSpeed or XSpeed then
;
MovesDelay = 8
else
;
MovesDelay = 0
endif
;
if MovesDelay > 0 then
;Find the magnitude of the speed vector
;This will be the distance that each foot moves with each step
DCoord = SQR(XSpeed * XSpeed + YSpeed * YSpeed)
;Find the cosine of the angle away from the forward axis
TmpCos = XSpeed * 127 / DCoord
;Use the ACos subroutine to find the angle in degrees
gosub ACos
;Set the found angle to the directional angle
DAngle = TmpAngle
;If the YSpeed is positive then reverse the angle
if YSpeed > 0 then
DAngle = 256 - DAngle
endif
;This ensures that the speed (movement) vector is not too large
DCoord = DCoord max (128 - ABS(TibiaAngle * 2))
;
Steps = Steps + 1
;
if Steps > NbSteps then
;Resets Steps to 1
Steps = 1
;Toggles FlipFlap
FlipFlap = FlipFlap ^ 1
;NbSteps holds the value of GaitSpeed
NbSteps = GaitSpeed
endif
;
StepFlag = NbSteps - Steps + 1
;The new feet positions are then calculated
for Index = 0 to 5
XPos2(Index) = -(DCoord * COS(DAngle + (Index * 43 + 21)) / 300) ; 43 => 60 degrees
ZPos2(Index) = -(DCoord * SIN(DAngle + (Index * 43 + 21)) / 300) ; 43 => 60 degrees
next
;Tripod is assigned the value of FlipFlap
Tripod = FlipFlap
;The down positions are calculated for one of the tripods
gosub AorB_Down
;
Freeze = Steps < NbSteps
;Tripod is assigned to the opposite value of FlipFlap
Tripod = FlipFlap ^ 1
;The up positions are calculated for one of the tripods
gosub AorB_Up
;Next for all six legs
for Index = 0 to 5
;Distance and HipH_Angle with XZ
;Here is the distance that the foot will be traveling
Distance = SQR((XPos(Index) * XPos(Index) + ZPos(Index) * ZPos(Index)) / 4) * 2
;Here the angle is calculated and scaled to work with the motors
TmpCos = ZPos(Index) * 127 / Distance
;Since our TibiaAngle is 0 this doesn't change it
Distance = Distance + TibiaAngle ; Tibia angle for special legs (3DOF-C)
;This subroutine gives the actual angle and puts it in TmpAngle
Gosub ACos
;The angle has it's limits checked
HipH_Angle = (TmpAngle max HipH_AngleMax) min HipH_AngleMin
;----------
;Set Angle
;Copies of the X, Y, & Z positions are made along with a copy of the distance length
TmpXPos = XPos(Index) * 127
TmpYPos = (YPos(Index) * 127) max 0
TmpZPos = ZPos(Index) * 127
TmpDistance = ((Distance - HipV_HipH) * 127) min 0
;I have no idea what SEW stands for...
;This is a scaled version of the sum of the squares of the distance and the Y position
TmpSEWSEW = (TmpYPos * TmpYPos + TmpDistance * TmpDistance) / 16129
;This square root is the distance from the top of the leg to the foot (in the plane of that leg)
TmpSEW = SQR(TmpSEWSEW)
;Using the law of cosines (C^2 = A^2 + B^2 - 2ABcos(angle)) the knee angle is found
;Knee_Angle = ARCCOS( (Femur_Length^2 + Tibia_Length^2 - TmpSEW^2)/(2 * Femur_Length * Tibia_Length) * 127 )
;The 127 scales it to corrispond with the motor
TmpCos = (Femur_Length * Femur_Length + Tibia_Length * Tibia_Length - TmpSEWSEW)
TmpCos = TmpCos * 127 / (2 * Femur_Length * Tibia_Length)
gosub ACos
;The Knee_Angle is then bounds checked
Knee_Angle = (TmpAngle max Knee_AngleMax) min Knee_AngleMin
;This is the angle between the TmpSEW and the plane parallel to the ground
;This is calculated to be combined with the angle between TmpSEW and the Femur
; so only the angle above the plane parallel to the ground is the HipV_Angle
TmpCos = -TmpYPos / TmpSew
gosub ACos
HipV_Angle = TmpAngle
;Here is the law of cosines used again to calculate the agle between TmpSEW and the Femur
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
;----------
;The final HipH_Angle is the calculated angle (from above) plus the amount of rotation
HipH_Angle = HipH_Angle + Rotate(Index)
;Here the angles are converted (via a proportion) into motor positions
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
;Here is the actual walking instructions. There is a lot of calculation that goes into this...
;The "3000 - " offsets are added in for the motors that need to be reversed
serout SSC32,I8N1_38400,"#",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), |
"T180",13]
else
;Else occurs only when MovesDelay is less than zero
nap 3
if MovesDelay < -17 then
;
MovesDelay = NbSteps - Steps + 1
EndIf
endif
;Decriment MovesDelay
MovesDelay = MovesDelay - 1
goto main
;--------------------------------------------------------------------
;-------------Sub H3 Init
;This subroutine initializes the H3 engine to a specific state
H3Init
;NbSteps is initalized to 4
NbSteps = 4
;GaitSpeedTmp is also initalized to 4
GaitSpeedTmp = NbSteps
;This initalizes how high the legs rais when walking
LegUpShift = 35
;These are the cartisian coordinates of the end of the leg
for Index = 0 to 5
;X is the length of the hip and the length of the femur
;It is the distance away from the body (positive is further outward, negative is closer)
XPos(Index) = HipV_HipH + Femur_Length
;Y is the length of the tibia
;It is the distance below the body (positive is upward, negative is downward)
YPos(Index) = - Tibia_Length
;Z is the position left and right
;It is unclear as to which way is positive and negative
ZPos(Index) = 0
next
;Another subroutine is called
Gosub InitPos
;
MovesDelay = 8
; then All1500
;-------------Sub All1500
; This is still part of the initilization phase but it can be called on it’s own
All1500
;This sends all of the motors to their middle position
for Index = 0 to 27 ;preserve servo 28 (Deck Tilt),29 and 30 (Little Gripper),31 (Deck)
serout SSC32,I8N1_38400,"#",DEC Index,“P1500”]
next
;Here is the time for it to complete the last move and the return carriage (13) that starts the move
serout SSC32,I8N1_38400,"T576",13]
;A nap command lets the processor idle for a specific amount of time (5 = 576 ms)
nap 5
;Steps is set to NbSteps which is representitave of the gait speed
Steps = NbSteps
return
;-------------Sub InitPos
;Called during the initialization
InitPos
;These positons lower the body by raising the veritical hip positions and lowering the knees
serout SSC32,I8N1_38400,"#",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]
;Lets the processor idle while the motors move
nap 5
;Sets all of the horizontal motor positions to 1500
serout SSC32,I8N1_38400,"#",RRHH,RRHH2,“P1500#”,MRHH,MRHH2,“P1500#”,FRHH,FRHH2,“P1500#”, |
RLHH,RLHH2,“P1500#”,MLHH,MLHH2,“P1500#”,FLHH,FLHH2,“P1500T576”,13]
;Lets the processor idle while the motors move
nap 5
return
;-------------Sub Arc Cosinus
;I do believe that this should be Cosine… Cosinus?
ACos
;The max function returns the smaller expression just as the min returns the larger
;TmpCos is set beforehand (like a parameter for this function)
TmpCos = (TmpCos max 127) min -127
;If TmpCos is negative
if TmpCos < 0 then
;The read command reads one byte from EEPROM, it’s syntax is “read address variable”
;This will read the positive location TmpCos and put it’s value in TmpAngle
read -TmpCos,TmpAngle
;TmpAngle is not quite reversed but it is offset
TmpAngle = 127 - TmpAngle
else ;If TmpCos is positive then read out that data
read TmpCos,TmpAngle
endif
return
;-------------Tripod ‘A’ (tripod = 0) or ‘B’ (Tripod = 1) Down
;This creates the new cartisian coordinates in a down position for the tip of each leg in the given tripod
;Note: Tripod is a parameter of this fuction it must be set beforehand to get the requested results
AorB_Down
;The for loop that will either use the indexes 0,2,4 or 1,3,5 depending on what Tripod is set too
;Note: The Step 2 indicates that it should incriment by two each time
for Index = 0 + Tripod to 4 + Tripod Step 2
;The KneeShift is how high the leg is offset when it moves
KneeShift(Index) = 0
;Here the new cartisian coordinates are created
;Note: The StepFlag seems to be scaling the movements; it is related to how far the legs move
Rotate(Index) = Rotate(Index) + (Steering/5 - Rotate(Index))/StepFlag
YPos(Index) = -Tibia_Length
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
;This creates the new cartisian coordinates in an up position for the tip of each leg in the given tripod
;Note: Tripod is a parameter of this fuction it must be set beforehand to get the requested results
AorB_Up
;
StepFlag = StepFlag - 1
;The for loop that will either use the indexes 0,2,4 or 1,3,5 depending on what Tripod is set too
;Note: The Step 2 indicates that it should incriment by two each time
for Index = 0 + Tripod to 4 + Tripod Step 2
;
if (Steps > 1) and (Steps < (NbSteps - 1)) then
;This value is set by the start button
KneeShift(Index) = KneeShift(6)
else
;Otherwise keep the legs straight
KneeShift(Index) = 0
endif
;Here the new cartisian coordinates are created
;Note: The StepFlag seems to be scaling the movements; it is related to how far the legs move
Rotate(Index) = Rotate(Index) + (-Steering/5 - Rotate(Index))/StepFlag
YPos(Index) = -Tibia_Length + (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
;--------------------------------------------------------------------
[/code]
Compiling this gives me hundreds of errors first about “Overwriting previous address contents” with respect to the Basic Micro lib. i.e. Error[118] C:\Program Files\BasicMicro Inc\BasicATOM IDE\system\mbasic14\mbasicmac.lib 704 : Overwriting previous address contents.
and things like:
Error[116] C:\Program Files\BasicMicro Inc\BasicATOM IDE\system\mbasic14\mbasicadin.lib 334 : Value of symbol “_ADIN_CHANNEL0” differs on second pass
pass 1=2000, pass 2=2016 :
Also every label that I have in my program (even the ones the compiler generates) is apparently different on the second pass.
Error[116] C:\DOCUME~1\THISPC~1\DESKTOP\HEXAPO~1\AUTONO~1.ASM 413 : Value of symbol “MAIN” differs on second pass
pass 1=2644, pass 2=2660 :
From what I know about compilers they read through the program several times looking for errors first with a lexical analysis then syntax analysis etc. However I’m not sure how these errors can come about. If I remove the ADIN line then it compiles and works fine.
Note: Originally I wanted to use the force sensing resistors as digital inputs but the robot wasn’t heavy enough to consistently push them down and I didn’t want to risk having uncontrolled digital inputs that can go over 5 volts. So I decided to try and ask about these errors since the bot board should be able to handle this function.
Thank you for helping…