#RightStepUpDown04.sh
#for 24rpm ankle motors
0,StartLogging 
0,LogScriptFileName
#StartTimeInMS,HoldAngleWithMotor(AccelName,AccelAngleXYorZ,TargetAngle,DegreesPlus,DegreesMinus,MotorName,Dir*Strength,Duration/TimeOut,flags)

#***legs spread 12cm from left foot to right foot center*** - otherwise stage 2 does not work

#constant thrust step right
#no thrust analysis, but does check for angle

0,StoreAngle(ACCEL_LEFT_LOWER_LEG,X,LLLX0)  #-11.8
0,StoreAngle(ACCEL_RIGHT_LOWER_LEG,X,RLLX0)
0,StoreAngle(ACCEL_LEFT_LOWER_LEG,Z,LLLZ0)
0,StoreAngle(ACCEL_RIGHT_LOWER_LEG,Z,RLLZ0)
0,StoreAngle(ACCEL_TORSO,Z,TZ0)
0,StoreAngle(ACCEL_TORSO,X,TX0)
0,StoreAngle(ACCEL_HIP,X,HX0)
0,StoreAngle(ACCEL_RIGHT_UPPER_LEG,X,RULX0)
0,StoreAngle(ACCEL_RIGHT_UPPER_LEG,Z,RULZ0)
0,StoreAngle(ACCEL_LEFT_UPPER_LEG,X,LULX0)
0,StoreAngle(ACCEL_LEFT_UPPER_LEG,Z,LULZ0)

##Stage 1: LEAN LEFT - needs to be somewhat fast, and leaves a little momentum
##just to make sure robot is leaning at ankle before large thrust of side motors- otherwise side motors turn strongly and robot does not lean- left side rack rises up- but robot ankles have not moved
#1,MotorAccel(MOTOR_LEFT_SIDE,3,5000,ACCEL_LEFT_LOWER_LEG,Z,-4,-1,1,WAIT_STAGE|MIN_VELOCITY=5|MAX_VELOCITY=8|MAX_THRUST=3|MAX_REVERSE_THRUST=5|MAINTAIN_THRUST)
#1,MotorAccel(MOTOR_RIGHT_SIDE,3,5000,ACCEL_LEFT_LOWER_LEG,Z,-4,-1,1,WAIT_STAGE|HOLD_ANGLE|MIN_VELOCITY=5|MAX_VELOCITY=8|MAX_THRUST=3|MAX_REVERSE_THRUST=5|MAINTAIN_THRUST)
#1,MotorAccel(MOTOR_LEFT_ANKLE,3,5000,ACCEL_LEFT_FOOT,Z,0,-10,5,ACCEL_LEFT_LOWER_LEG,Z,-4,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=3|MAX_REVERSE_THRUST=8|MIN_VELOCITY=5|MAX_VELOCITY=8|FUTURE_LOG|MAINTAIN_THRUST)
#1,MotorAccel(MOTOR_RIGHT_ANKLE,3,5000,ACCEL_RIGHT_FOOT,Z,0,-10,5,ACCEL_LEFT_LOWER_LEG,Z,-4,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=3|MAX_REVERSE_THRUST=8|MIN_VELOCITY=5|MAX_VELOCITY=8|MAINTAIN_THRUST)
#1,MotorAccel(MOTOR_TORSO,3,5000,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=5|MAX_REVERSE_THRUST=3|MAINTAIN_THRUST|MAX_VELOCITY=5)



#stage 2: LEFT_SIDE reverses, to make torso Z straighten, and put torso weight over left leg, can be more positive than 1deg
#2,MotorAccel(MOTOR_LEFT_SIDE,6,3000,ACCEL_TORSO,Z,1,-1,4,WAIT_STAGE|MAX_THRUST=7)
#inst lasts until stage 4 (balance with foot in air) Roll degrees 1 to -5
#2,MotorAccel(MOTOR_LEFT_SIDE,6,3000,ACCEL_TORSO,Z,-2,-2,1,WAIT_STAGE|MAX_THRUST=9|ABORT_ON_TIMEOUT)
#if LLLZ goes too negative - falls to left, LSM should be +, LLLZ goes positive falls to right, LSM should be -
# high max velocity so that it is not afraid to increase thrust - removed MAINTAIN_THRUST|
#change LSM MAX_THRUST from 3 to 5 to speed up step
#note that side motors need MaxThrust0=3 for ankle while MaxThrust1=6 for torso (once robot is leaning far enough to the left)
2,MotorAccel(MOTOR_LEFT_SIDE,4,4000,ACCEL_LEFT_LOWER_LEG,Z,-7,-1.5,1.5,ACCEL_TORSO,Z,-5,-2,1,WAIT_STAGE|MAX_THRUST0=4|MAX_REVERSE_THRUST0=4|MIN_VELOCITY0=8|MAX_VELOCITY0=10|MAX_THRUST1=7|MAX_REVERSE_THRUST1=5|MIN_VELOCITY1=10|MAX_VELOCITY1=30|FUTURE_LOG|NO_STOP_ON_TARGET|ABORT_ON_TIMEOUT)
#
#2,MotorAccel(MOTOR_RIGHT_SIDE,3,5000,ACCEL_LEFT_LOWER_LEG,Z,-5,-3,0,WAIT_STAGE|HOLD_ANGLE|MIN_VELOCITY=10|MAX_VELOCITY=14|MAX_THRUST=3|MAX_REVERSE_THRUST=5)
2,MotorAccel(MOTOR_RIGHT_SIDE,4,4000,ACCEL_LEFT_LOWER_LEG,Z,-7,-1.5,1.5,ACCEL_TORSO,Z,-5,-5,3,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST0=4|MAX_REVERSE_THRUST0=4|MIN_VELOCITY0=8|MAX_VELOCITY0=10|MAX_THRUST1=7|MAX_REVERSE_THRUST1=5|MIN_VELOCITY1=10|MAX_VELOCITY1=30|NO_STOP_ON_TARGET)
#|NO_STOP_ON_TARGET
#NO_STOP_ON_TARGET - is needed because otherwise side motor may reverse back in an effort to brake
#2,MotorAccel(MOTOR_LEFT_SIDE,5,4000,ACCEL_TORSO,Z,-4,-5,3,WAIT_STAGE|MAX_THRUST=12|ABORT_ON_TIMEOUT|MAX_VELOCITY=30|MIN_VELOCITY=5)
#ABORT_ANGLE0=5 - end script if ankle Z turns past 5 degrees was -6
#  ***Important for the Left Ankle to have a large negative LLLZ range, because the side will make LLLZ positive and if the ankle
# is thrusting in that direction- it builds up too much velocity
#oppose=7.0:0.5 - important for LLLZ not to have a positive velocity at end of stage 2
#2,MotorAccel(MOTOR_LEFT_ANKLE,3,4000,ACCEL_LEFT_FOOT,Z,0,-20,10,ACCEL_LEFT_LOWER_LEG,Z,-8,-2,0,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MAX_VELOCITY=5|ABORT_ANGLE0=30|MAINTAIN_THRUST|OPPOSE_MOTION=7.0:1.0|FUTURE_LOG)
#2,MotorAccel(MOTOR_LEFT_ANKLE,3,4000,ACCEL_LEFT_FOOT,Z,0,-20,10,ACCEL_LEFT_LOWER_LEG,Z,-7.5,-1,0,WAIT_STAGE|HOLD_ANGLE=10.0:1.0|MAX_THRUST=20|MAX_REVERSE_THRUST=4|MIN_VELOCITY=7|MAX_VELOCITY=10|ABORT_ANGLE0=30|MAINTAIN_THRUST|FUTURE_LOG)
#***NO HOLD_ANGLE***
#if V=N (V>0), in beginning, bu V is <1, use V*3 to make sure thrust is high enough to lean robot
2,MotorAccel(MOTOR_LEFT_ANKLE,4,4000,ACCEL_LEFT_FOOT,Z,0,-20,10,ACCEL_LEFT_LOWER_LEG,Z,-7,-0.5,0.5,WAIT_STAGE|MAX_THRUST=5|MAX_REVERSE_THRUST=5|MIN_VELOCITY=8|MAX_VELOCITY=10|ABORT_ANGLE0=30|HOLD_ANGLE=20:5|MAINTAIN_THRUST|FUTURE_LOG)
2,MotorAccel(MOTOR_RIGHT_ANKLE,4,4000,ACCEL_RIGHT_FOOT,Z,0,-10,5,ACCEL_LEFT_LOWER_LEG,Z,-7,-0.5,0.5,WAIT_STAGE|HOLD_ANGLE=20:5|MAX_THRUST=5|MAX_REVERSE_THRUST=5|MIN_VELOCITY=8|MAX_VELOCITY=10)
#|NO_STOP_ON_TARGET
#legs hold up torso
#2,MotorAccel(MOTOR_LEFT_LEG,MOTOR_RIGHT_LEG,3,5000,ACCEL_TORSO,X,TX0,-1,1,WAIT_STAGE|HOLD_ANGLE|DOUBLE_THRUST|MAX_THRUST=6)
#knee holds up upper legs through last stage
#2,MotorAccel(MOTOR_LEFT_KNEE,3,10000,ACCEL_LEFT_UPPER_LEG,X,CurrentAngle[LULX],-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=9|MAX_REVERSE_THRUST=6|LAST_STAGE=4|ABORT_ANGLE0=25)
#2,MotorAccel(MOTOR_LEFT_KNEE,3,4000,ACCEL_LEFT_UPPER_LEG,X,CurrentAngle[LULX],-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=9|MAX_REVERSE_THRUST=6|MAINTAIN_THRUST|ABORT_ANGLE0=25)
#2,MotorAccel(MOTOR_LEFT_KNEE,3,4000,ACCEL_TORSO,Z,-5,-2,2,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=5|MAX_REVERSE_THRUST=2|MAINTAIN_THRUST|MAX_VELOCITY=15|MIN_VELOCITY=5|ABORT_ANGLE0=25)
#2,MotorAccel(MOTOR_RIGHT_KNEE,3,4000,ACCEL_RIGHT_UPPER_LEG,X,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=8)
#feet hold up lower legs
2,MotorAccel(MOTOR_LEFT_FOOT,3,4000,ACCEL_LEFT_LOWER_LEG,X,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE=0.25:0.5|MAX_THRUST=7|MAX_REVERSE_THRUST=3|MAX_VELOCITY=10)
2,MotorAccel(MOTOR_RIGHT_FOOT,3,4000,ACCEL_RIGHT_LOWER_LEG,X,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE=0.25:0.5|MAX_THRUST=7|MAX_REVERSE_THRUST=3|MAX_VELOCITY=10)
2,MotorAccel(MOTOR_TORSO,3,4000,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=2|MAX_REVERSE_THRUST=2|MAINTAIN_THRUST|MAX_VELOCITY=10|OPPOSE_MOTION=0.25:0.75)


#Stage 3: LIFT RIGHT LEG only (no knee or foot movement) 
#(LEFT FOOT MOTOR HOLDS ROBOT FROM FALLING FORWARD)
#todo: allow MAINTAIN_THRUST flag to work on MOTOR_RIGHT_LEG  +3 because can skip from 19 to 21
#was 16/18 slower is 12/14
3,MotorAccel(MOTOR_RIGHT_LEG,12,600,ACCEL_RIGHT_UPPER_LEG,X,20,-1,5,WAIT_STAGE|CONSTANT_THRUST)
3,MotorAccel(MOTOR_LEFT_LEG,12,600,ACCEL_RIGHT_UPPER_LEG,X,20,-1,1,WAIT_STAGE|CONSTANT_THRUST|HOLD_ANGLE)
#bend right knee so foot lifts in place - decrease angle X (lower leg rotates forward)
3,MotorAccel(MOTOR_RIGHT_KNEE,14,600,ACCEL_RIGHT_LOWER_LEG,X,-20,-1,1,WAIT_STAGE|CONSTANT_THRUST|HOLD_ANGLE)
#left knee bring upper leg from 6 to 10 (was LULX)
#3,MotorAccel(MOTOR_LEFT_KNEE,3,600,ACCEL_LEFT_UPPER_LEG,X,10,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=9|MAX_REVERSE_THRUST=6|MAINTAIN_THRUST|ABORT_ANGLE0=25)
#in stage 3 left knee generally should not push torso right
#left_knee on torso z has problem here- causes LULZ to fall too far right when thrusting -
#3,MotorAccel(MOTOR_LEFT_KNEE,10,600,ACCEL_TORSO,Z,-4,-1,1,WAIT_STAGE|HOLD_ANGLE=0.1:0.1|MAX_THRUST=6|MAX_REVERSE_THRUST=2|MAX_VELOCITY=20|MAINTAIN_THRUST|ABORT_ANGLE0=25)
#bias left knee for pos
#3,MotorAccel(MOTOR_LEFT_KNEE,3,600,ACCEL_LEFT_UPPER_LEG,X,10,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=9|MAX_REVERSE_THRUST=6|MAINTAIN_THRUST|ABORT_ANGLE0=25|OPPOSE_MOTION=0.25:2.0)
3,MotorAccel(MOTOR_LEFT_KNEE,3,600,ACCEL_LEFT_UPPER_LEG,X,LULX0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=9|MAX_REVERSE_THRUST=6|MAINTAIN_THRUST|ABORT_ANGLE0=25)
#OPPOSE_MOTION=0.5:0.5
#LKM constant thrust
#3,MotorAccel(MOTOR_LEFT_KNEE,4,600,ACCEL_LEFT_UPPER_LEG,X,20,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=10|MAX_REVERSE_THRUST=6|MAINTAIN_THRUST|ABORT_ANGLE0=25|CONSTANT_THRUST)
#separate inst for left angle in stage 3 because stage 3 (lifting right leg) causes the main pull on the ankle- not stage 2 as much
#and a stronger ankle is needed in stage 3 and 4
#not a big deal for left ankle thrust to =0 at the start of stage 3
#3,MotorAccel(MOTOR_LEFT_ANKLE,3,10000,ACCEL_LEFT_FOOT,Z,0,-10,10,ACCEL_LEFT_LOWER_LEG,Z,-4,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=12|MAX_REVERSE_THRUST=3|ABORT_ANGLE0=15|LAST_STAGE=4)
#3,MotorAccel(MOTOR_LEFT_ANKLE,12,600,ACCEL_LEFT_FOOT,Z,0,-15,15,ACCEL_LEFT_LOWER_LEG,Z,-7,-2,0,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MAX_VELOCITY=5|ABORT_ANGLE0=30|MAINTAIN_THRUST|OPPOSE_MOTION=3.0:0.5|FUTURE_LOG)
#3,MotorAccel(MOTOR_LEFT_ANKLE,12,600,ACCEL_LEFT_LOWER_LEG,Z,-7,-2,0,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MAX_VELOCITY=5|MAINTAIN_THRUST|OPPOSE_MOTION=4.0:0.1|FUTURE_LOG)
3,MotorAccel(MOTOR_LEFT_ANKLE,12,600,ACCEL_LEFT_LOWER_LEG,Z,-7,-1,0,WAIT_STAGE|HOLD_ANGLE=10.0:2.0|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MAX_VELOCITY=5|MAINTAIN_THRUST|ABORT_ANGLE0=30|FUTURE_LOG)
#important that left knee hold ULX to 17 standing is 10 - left knee needs to push hard initially
#3,MotorAccel(MOTOR_LEFT_SIDE,6,2000,ACCEL_LEFT_LOWER_LEG,Z,-5,-1,1,ACCEL_TORSO,Z,-2,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=9|MAX_REVERSE_THRUST=2|LAST_STAGE=4)
#3,MotorAccel(MOTOR_LEFT_SIDE,6,2000,ACCEL_LEFT_LOWER_LEG,Z,-5,-1,1,ACCEL_TORSO,Z,-2,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=9|MAX_REVERSE_THRUST=2|MAINTAIN_THRUST)
#change range so LSM is going to be constantly thrusting and pulling the RL up (-9 -4 -1) to (-9 -4 -3)
#MaxVelocity=20 because left side needs to trust positively immediately
#3,MotorAccel(MOTOR_LEFT_SIDE,10,600,ACCEL_TORSO,Z,-4,-5,0,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MAX_VELOCITY=20|MAINTAIN_THRUST|OPPOSE_MOTION_MAINTAIN=4.0:0.05|FUTURE_LOG)
#if Torso Z is ok, left side motor helps RSM hold RULZ
#3,MotorAccel(MOTOR_LEFT_SIDE,10,600,ACCEL_TORSO,Z,-4,-5,0,ACCEL_RIGHT_UPPER_LEG,Z,-3,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MAX_VELOCITY=20|MAINTAIN_THRUST|OPPOSE_MOTION_MAINTAIN=4.0:0.5|FUTURE_LOG)
#strive for -7 using Thrust=20 because otherwise left side falls too soon, and right leg falls inward
#note that LSM is currently not helping LAM and that should be changed
3,MotorAccel(MOTOR_LEFT_SIDE,20,600,ACCEL_TORSO,Z,-8,-2,1,ACCEL_RIGHT_UPPER_LEG,Z,-13,-1,1,WAIT_STAGE|HOLD_ANGLE=10:0|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MIN_VELOCITY=5|MAX_VELOCITY=20|MAINTAIN_THRUST|NO_STOP_ON_TARGET|FUTURE_LOG)
#3,MotorAccel(MOTOR_LEFT_SIDE,20,600,ACCEL_RIGHT_UPPER_LEG,Z,-13,-1,1,ACCEL_TORSO,Z,-8,-2,1,WAIT_STAGE|HOLD_ANGLE=10:0|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MIN_VELOCITY=5|MAX_VELOCITY=20|MAINTAIN_THRUST|NO_STOP_ON_TARGET|FUTURE_LOG)
#3,MotorAccel(MOTOR_LEFT_SIDE,20,600,ACCEL_LEFT_LOWER_LEG,Z,-7,-2,1,ACCEL_RIGHT_UPPER_LEG,Z,-13,-1,1,WAIT_STAGE|HOLD_ANGLE=10:0|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MIN_VELOCITY=5|MAX_VELOCITY=20|MAINTAIN_THRUST|FUTURE_LOG)
#3,MotorAccel(MOTOR_LEFT_SIDE,20,600,ACCEL_TORSO,Z,-5,-5,1,ACCEL_LEFT_LOWER_LEG,Z,-6,-1,1,WAIT_STAGE|HOLD_ANGLE=10:0|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MIN_VELOCITY=5|MAX_VELOCITY=20|MAINTAIN_THRUST|FUTURE_LOG)
#LSM needs to watch left upper leg too- because it needs to hold a large positive thrust
#3,MotorAccel(MOTOR_LEFT_SIDE,10,600,ACCEL_TORSO,Z,-4,-5,0,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MAX_VELOCITY=20|MAINTAIN_THRUST|OPPOSE_MOTION_MAINTAIN=4.0:0.05|FUTURE_LOG)
#3,MotorAccel(MOTOR_LEFT_SIDE,10,600,ACCEL_LEFT_LOWER_LEG,Z,-7,-2,0,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MAX_VELOCITY=20|MAINTAIN_THRUST|OPPOSE_MOTION=2.0:0.5|FUTURE_LOG)
#it seems likely that right and left side need to be syncronized with the same thrust magnitude for z stability
#***do not use oppose motion on Right lower leg because it pushes the motor ccw, in the wrong direction***
#3,MotorAccel(MOTOR_LEFT_SIDE,10,600,ACCEL_RIGHT_LOWER_LEG,Z,-12,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=10|MAX_REVERSE_THRUST=2|MAX_VELOCITY=20|MAINTAIN_THRUST|FUTURE_LOG)
#right side pushes away from left leg - right leg tends to fall towards left leg in stage 3
#3,MotorAccel(MOTOR_RIGHT_SIDE,6,2000,ACCEL_RIGHT_LOWER_LEG,Z,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=5|MAX_REVERSE_THRUST=2|LAST_STAGE=4)
#3,MotorAccel(MOTOR_RIGHT_SIDE,10,2000,ACCEL_RIGHT_LOWER_LEG,Z,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=10|MAX_REVERSE_THRUST=2|MAINTAIN_THRUST)
#right side watches RLLZ was -12
#initial thrust is wrongly set to -1 why?- possibly DO_NOT_USE_GRAVITY flag
#OPPOSE_MOTION_MAINTAIN=2.0:0.5
#3,MotorAccel(MOTOR_RIGHT_SIDE,1,600,ACCEL_RIGHT_LOWER_LEG,Z,-12,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=10|MAX_REVERSE_THRUST=2|MAX_VELOCITY=10|MAINTAIN_THRUST|OPPOSE_MOTION=)
#3,MotorAccel(MOTOR_RIGHT_SIDE,10,600,ACCEL_RIGHT_UPPER_LEG,Z,-3,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MAX_VELOCITY=10|MAINTAIN_THRUST|OPPOSE_MOTION=4.0:0.0)
3,MotorAccel(MOTOR_RIGHT_SIDE,20,600,ACCEL_RIGHT_UPPER_LEG,Z,-12,-1,0,WAIT_STAGE|HOLD_ANGLE=10:0|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MAX_VELOCITY=10|MAINTAIN_THRUST)
#left foot brings body forward from -12 to -21 (was CurrentAngle[LLLX])
#3,MotorAccel(MOTOR_LEFT_FOOT,5,600,ACCEL_LEFT_FOOT,X,0,-2,2,ACCEL_LEFT_LOWER_LEG,X,-21,-1,1,WAIT_STAGE|HOLD_ANGLE|ABORT_ANGLE1=-30|MAX_THRUST=16|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20)
#3,MotorAccel(MOTOR_LEFT_FOOT,5,600,ACCEL_LEFT_FOOT,X,0,-2,2,ACCEL_LEFT_LOWER_LEG,X,-21,-1,1,WAIT_STAGE|HOLD_ANGLE|ABORT_ANGLE1=-30|MAX_THRUST=16|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20|OPPOSE_MOTION=4.0:0.1)
#3,MotorAccel(MOTOR_LEFT_FOOT,5,600,ACCEL_LEFT_FOOT,X,0,-10,10,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE=1:2|ABORT_ANGLE1=-30|MAX_THRUST=16|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20)
3,MotorAccel(MOTOR_LEFT_FOOT,5,600,ACCEL_LEFT_FOOT,X,0,-10,10,ACCEL_LEFT_LOWER_LEG,X,LLLX0,-1,1,WAIT_STAGE|HOLD_ANGLE=2:2|ABORT_ANGLE1=-30|MAX_THRUST=16|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20)
#right ankle hold Z at 0
#3,MotorAccel(MOTOR_RIGHT_ANKLE,10,600,ACCEL_RIGHT_FOOT,Z,0,-3,3,WAIT_STAGE|HOLD_ANGLE)
#right ankle needs to repeat stage 2 inst because the right foot is still on the ground for a long time in stage 3
3,MotorAccel(MOTOR_RIGHT_ANKLE,3,600,ACCEL_RIGHT_FOOT,Z,0,-10,5,ACCEL_LEFT_LOWER_LEG,Z,-8,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=3|MAX_REVERSE_THRUST=4|MIN_VELOCITY=10|MAX_VELOCITY=12)
3,MotorAccel(MOTOR_TORSO,3,10000,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.5:0.25|MAX_THRUST=10|MAX_REVERSE_THRUST=3|MAX_VELOCITY=10|MAINTAIN_THRUST)
#right foot should move RFX to 0 to decrease friction with the floor from the front of the right foot.
#rt foot is still on ground for part of stage 3- so better to have little to no movement
3,MotorAccel(MOTOR_RIGHT_FOOT,10,600,ACCEL_RIGHT_FOOT,X,0,-5,5,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=12|MAX_REVERSE_THRUST=3|MAX_VELOCITY=5)


#Stage 4: LOWER RIGHT LEG SWINGS BACK, UPPER LEG COMES BACK DOWN, left side holds right leg off the ground 
#***note no HOLD_ANGLE***
4,MotorAccel(MOTOR_RIGHT_LEG,10,800,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-1,1,WAIT_STAGE|CONSTANT_THRUST)
4,MotorAccel(MOTOR_LEFT_LEG,10,800,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=18|MAX_REVERSE_THRUST=3|CONSTANT_THRUST)
#right knee swings forward - needs HOLD_ANGLE upper leg motor can stop stage
4,MotorAccel(MOTOR_RIGHT_KNEE,13,800,ACCEL_RIGHT_LOWER_LEG,X,RLLX0,-1,1,WAIT_STAGE|CONSTANT_THRUST|HOLD_ANGLE|ABORT_ANGLE0=-30)
#4,MotorAccel(MOTOR_RIGHT_FOOT,1,400,ACCEL_RIGHT_FOOT,X,0,-1,1,WAIT_STAGE|HOLD_ANGLE) #does not use constant thrust
#left ankle still holds lean left
#4,MotorAccel(MOTOR_LEFT_ANKLE,3,400,ACCEL_LEFT_FOOT,Z,0,-15,15,ACCEL_LEFT_LOWER_LEG,Z,-7,-2,0,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=20|MAX_REVERSE_THRUST=2|ABORT_ANGLE0=20|MAINTAIN_THRUST|OPPOSE_MOTION=3.0:0.5)
#left ankle needs to release in stage 4 to allow body to fall right
#4,MotorAccel(MOTOR_LEFT_ANKLE,3,230,ACCEL_LEFT_LOWER_LEG,Z,-7,-2,0,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=20|MAX_REVERSE_THRUST=2|ABORT_ANGLE0=20|MAINTAIN_THRUST|OPPOSE_MOTION=3.0:1.0)
#hold left side up
#4,MotorAccel(MOTOR_LEFT_ANKLE,3,230,ACCEL_LEFT_LOWER_LEG,Z,-8,-1,0,WAIT_STAGE|HOLD_ANGLE=10:1|MAX_THRUST=20|MAX_REVERSE_THRUST=2|ABORT_ANGLE0=20|MAINTAIN_THRUST)
#vs bring left side back down
4,MotorAccel(MOTOR_LEFT_ANKLE,3,800,ACCEL_LEFT_FOOT,Z,0,-10,10,ACCEL_LEFT_LOWER_LEG,Z,-7,-1,0,WAIT_STAGE|HOLD_ANGLE=20:2|MAX_THRUST=20|MAX_REVERSE_THRUST=2|ABORT_ANGLE0=30|MAINTAIN_THRUST)
#4,MotorAccel(MOTOR_LEFT_SIDE,6,2000,ACCEL_LEFT_LOWER_LEG,Z,-5,-1,1,ACCEL_TORSO,Z,-2,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=9|MAX_REVERSE_THRUST=2|MAINTAIN_THRUST)
#Note there is a problem in stage 4 when the LLLZ starts to move back to the right (positive roll), the LeftSide motor needs to keep lifting the right leg, to reverse would quickly bring the right leg down. So for now, just link the LeftSide motor to the Torso Z 
#important for LSM to hold up right leg (-9 -4 -3)
#4,MotorAccel(MOTOR_LEFT_SIDE,6,230,ACCEL_TORSO,Z,-4,-5,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MAINTAIN_THRUST|OPPOSE_MOTION_MAINTAIN=4.0:0.0|FUTURE_LOG)
#if Torso Z is ok LSM helps keep right leg from falling
#4,MotorAccel(MOTOR_LEFT_SIDE,6,230,ACCEL_TORSO,Z,-4,-5,1,ACCEL_RIGHT_UPPER_LEG,Z,-3,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MAINTAIN_THRUST|OPPOSE_MOTION_MAINTAIN=4.0:0.0|FUTURE_LOG)
#strive for -7 because otherwise left side collapses, and right leg falls inward too
4,MotorAccel(MOTOR_LEFT_SIDE,6,800,ACCEL_TORSO,Z,-8,-2,1,ACCEL_RIGHT_UPPER_LEG,Z,-13,-1,1,WAIT_STAGE|HOLD_ANGLE=10:0|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MAINTAIN_THRUST|FUTURE_LOG)
#4,MotorAccel(MOTOR_LEFT_SIDE,6,800,ACCEL_TORSO,Z,-4,-5,1,ACCEL_LEFT_LOWER_LEG,Z,-6,-1,1,WAIT_STAGE|HOLD_ANGLE=10:0|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MAINTAIN_THRUST|FUTURE_LOG)
#|OPPOSE_MOTION_MAINTAIN=1.0:0.0
#4,MotorAccel(MOTOR_LEFT_SIDE,10,400,ACCEL_RIGHT_LOWER_LEG,Z,-14,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=10|MAX_REVERSE_THRUST=2|MAINTAIN_THRUST|FUTURE_LOG)
#4,MotorAccel(MOTOR_RIGHT_SIDE,10,2000,ACCEL_RIGHT_LOWER_LEG,Z,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=10|MAX_REVERSE_THRUST=2|MAINTAIN_THRUST)
#|OPPOSE_MOTION_MAINTAIN=1.0:0.5
#4,MotorAccel(MOTOR_RIGHT_SIDE,10,230,ACCEL_RIGHT_UPPER_LEG,Z,-3,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=20|MAX_REVERSE_THRUST=2|OPPOSE_MOTION=1.0:0.0)
4,MotorAccel(MOTOR_RIGHT_SIDE,10,800,ACCEL_RIGHT_UPPER_LEG,Z,-12,-1,0,WAIT_STAGE|HOLD_ANGLE=10:0|MAX_THRUST=20|MAX_REVERSE_THRUST=2)
#4,MotorAccel(MOTOR_RIGHT_SIDE,10,300,ACCEL_RIGHT_LOWER_LEG,Z,-14,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=20|MAX_REVERSE_THRUST=2)
#lean torso forward from -12 to -15 (was TX0) was -15
4,MotorAccel(MOTOR_TORSO,3,800,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=16|MAX_REVERSE_THRUST=3|MAX_VELOCITY=30|MAINTAIN_THRUST)
#4,MotorAccel(MOTOR_LEFT_LEG,10,2000,ACCEL_RIGHT_UPPER_LEG,X,17,-1,1,WAIT_STAGE|MAINTAIN_THRUST|MAX_THRUST=8|MAX_REVERSE_THRUST=3|HOLD_ANGLE)
#Left Leg Motor holds hip (or torso)
#4,MotorAccel(MOTOR_LEFT_LEG,10,400,ACCEL_HIP,X,HX0,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=8|MAX_REVERSE_THRUST=3|MAINTAIN_THRUST)
#4,MotorAccel(MOTOR_LEFT_LEG,10,600,ACCEL_RIGHT_UPPER_LEG,X,20,-1,1,WAIT_STAGE|CONSTANT_THRUST|HOLD_ANGLE)
#possibly want Left Leg to look at torso and help to push robot upper weight forward (was -15)
#4,MotorAccel(MOTOR_LEFT_LEG,14,800,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=18|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20)
#4,MotorAccel(MOTOR_LEFT_LEG,14,800,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=18|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20)
#right leg motor holds up right leg so lower leg and foot can clear in swinging right lower leg forward
#changed angle from 20 to 30 so right leg can keep pushing up as right lower leg swings forward
#4,MotorAccel(MOTOR_RIGHT_LEG,10,230,ACCEL_RIGHT_UPPER_LEG,X,30,-1,1,WAIT_STAGE|CONSTANT_THRUST|HOLD_ANGLE)
#LFM needs to bring body forward, from -12 to -21 was LLLX0
#4,MotorAccel(MOTOR_LEFT_FOOT,10,230,ACCEL_LEFT_FOOT,X,0,-5,5,ACCEL_LEFT_LOWER_LEG,X,-21,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=16|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20)  was -15
#4,MotorAccel(MOTOR_LEFT_FOOT,10,800,ACCEL_LEFT_FOOT,X,0,-5,5,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE=2:0.5|MAX_THRUST=16|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20)
#note subtle point- hip falls forward, but torso does not because torso motor fixes the Torso X
#4,MotorAccel(MOTOR_LEFT_FOOT,10,800,ACCEL_LEFT_FOOT,X,0,-5,5,ACCEL_HIP,X,0,-1,1,WAIT_STAGE|HOLD_ANGLE=1:2|MAX_THRUST=16|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20)
#use 2:2 because can fall backwards or forwards and needs more thrust
4,MotorAccel(MOTOR_LEFT_FOOT,10,800,ACCEL_LEFT_FOOT,X,0,-10,10,ACCEL_LEFT_LOWER_LEG,X,LLLX0,-1,1,WAIT_STAGE|HOLD_ANGLE=2:2|MAX_THRUST=16|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20)
#|ABORT_ANGLE1=-30  make -2 2 range?
#left knee needs to shift weight back to left from 3/5->12, (was: bring upper leg from 6 to 3 (was LULX) MAX_THRUST=12)
#LKM oppose with bias against negative motion (in favor of knee compressing- pulling robot weight to left)
#4,MotorAccel(MOTOR_LEFT_KNEE,10,230,ACCEL_LEFT_UPPER_LEG,X,10,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=16|MAX_REVERSE_THRUST=6|MAINTAIN_THRUST|ABORT_ANGLE0=25)
#4,MotorAccel(MOTOR_LEFT_KNEE,10,800,ACCEL_TORSO,Z,-4,-2,2,WAIT_STAGE|HOLD_ANGLE=0.3:0.1|MAX_THRUST=12|MAX_REVERSE_THRUST=6|MAINTAIN_THRUST|ABORT_ANGLE0=25)
#4,MotorAccel(MOTOR_LEFT_KNEE,10,800,ACCEL_HIP,Z,0,-2,2,ACCEL_LEFT_UPPER_LEG,X,LULX0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=12|MAX_REVERSE_THRUST=6|MAINTAIN_THRUST|ABORT_ANGLE0=30)
4,MotorAccel(MOTOR_LEFT_KNEE,10,800,ACCEL_LEFT_UPPER_LEG,X,LULX0,-2,2,ACCEL_HIP,Z,0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=12|MAX_REVERSE_THRUST=6|MAINTAIN_THRUST|ABORT_ANGLE0=30)
#4,MotorAccel(MOTOR_LEFT_KNEE,10,800,ACCEL_HIP,Z,0,-2,2,ACCEL_LEFT_UPPER_LEG,X,LULX0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.3:0.1|MAX_THRUST=12|MAX_REVERSE_THRUST=6|MAINTAIN_THRUST|ABORT_ANGLE0=30)
#bias for pos
#4,MotorAccel(MOTOR_LEFT_KNEE,10,400,ACCEL_LEFT_UPPER_LEG,X,10,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=16|MAX_REVERSE_THRUST=6|MAINTAIN_THRUST|ABORT_ANGLE0=25|OPPOSE_MOTION=0.2:2.0)
#LKM constant thrust
#4,MotorAccel(MOTOR_LEFT_KNEE,4,400,ACCEL_LEFT_UPPER_LEG,X,20,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=10|MAX_REVERSE_THRUST=6|MAINTAIN_THRUST|ABORT_ANGLE0=25|CONSTANT_THRUST)
#right ankle and right foot motors should straighten right foot
4,MotorAccel(MOTOR_RIGHT_ANKLE,10,800,ACCEL_RIGHT_FOOT,Z,0,-2,2,WAIT_STAGE|HOLD_ANGLE=1:1|MAX_THRUST=12|MAX_REVERSE_THRUST=6)
4,MotorAccel(MOTOR_RIGHT_FOOT,10,800,ACCEL_RIGHT_FOOT,X,0,-2,2,WAIT_STAGE|HOLD_ANGLE=1:1|MAX_THRUST=12|MAX_REVERSE_THRUST=6)

#Stage 5: BALANCE stand for 3 seconds
#(Basically try to lean back to the center and hold those angles)
#sides
#5,MotorAccel(MOTOR_LEFT_SIDE,MOTOR_RIGHT_SIDE,3,3000,ACCEL_TORSO,Z,0,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=8|MAX_REVERSE_THRUST=3|DOUBLE_THRUST)
#important that side motors work to keep torso Z=0- otherwise robot may fall sideways (was: set both side motors based on lower leg Z to go back to starting Z angle)
#5,MotorAccel(MOTOR_LEFT_SIDE,3,3000,ACCEL_LEFT_LOWER_LEG,Z,LLLZ0,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=8|MAX_REVERSE_THRUST=3|DOUBLE_THRUST)
#5,MotorAccel(MOTOR_RIGHT_SIDE,3,3000,ACCEL_RIGHT_LOWER_LEG,Z,RLLZ0,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=8|MAX_REVERSE_THRUST=3|DOUBLE_THRUST)
#todo: watch upper or lower leg too
#important to have all upper motors focus on torso, since the torso has a lot of weight and is the biggest factor in the robot falling forward/back  or left/right
#important to have side motors look at first: Lower Leg Z, second: torso Z
#5,MotorAccel(MOTOR_LEFT_SIDE,3,3000,ACCEL_TORSO,Z,0,-2,2,ACCEL_LEFT_LOWER_LEG,Z,LLLZ0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=18|MAX_REVERSE_THRUST=3)
5,MotorAccel(MOTOR_LEFT_SIDE,3,3000,ACCEL_LEFT_UPPER_LEG,Z,LULZ0,-2,2,ACCEL_TORSO,Z,0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.2:0.2|MAX_THRUST=7|MAX_REVERSE_THRUST=3)
#5,MotorAccel(MOTOR_RIGHT_SIDE,3,3000,ACCEL_TORSO,Z,0,-2,2,ACCEL_RIGHT_LOWER_LEG,Z,RLLZ0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=18|MAX_REVERSE_THRUST=3)
5,MotorAccel(MOTOR_RIGHT_SIDE,3,3000,ACCEL_RIGHT_UPPER_LEG,Z,RULZ0,-2,2,ACCEL_TORSO,Z,0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.2:0.2|MAX_THRUST=7|MAX_REVERSE_THRUST=3)
#5,MotorAccel(MOTOR_LEFT_SIDE,3,3000,ACCEL_TORSO,Z,0,-2,2,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=10|MAX_REVERSE_THRUST=3|OPPOSE_MOTION=1.0:0.25)
#5,MotorAccel(MOTOR_RIGHT_SIDE,3,3000,ACCEL_TORSO,Z,0,-2,2,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=10|MAX_REVERSE_THRUST=3|OPPOSE_MOTION=1.0:0.25)
#5,MotorAccel(MOTOR_LEFT_SIDE,3,3000,ACCEL_LEFT_LOWER_LEG,Z,LLLZ0,-2,2,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=18|MAX_REVERSE_THRUST=3|OPPOSE_MOTION=1.0:1.0)
#5,MotorAccel(MOTOR_RIGHT_SIDE,3,3000,ACCEL_RIGHT_LOWER_LEG,Z,RLLZ0,-2,2,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=18|MAX_REVERSE_THRUST=3|OPPOSE_MOTION=1.0:1.0)
#legs - both try to hold torso X to starting angle
#was TX0 (-12)  max-thrust=8 = 4 each
#perhaps legs should try to hold both torso and upper/lower leg
#5,MotorAccel(MOTOR_LEFT_LEG,MOTOR_RIGHT_LEG,3,3000,ACCEL_TORSO,X,-15,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=8|MAX_REVERSE_THRUST=3|DOUBLE_THRUST)
5,MotorAccel(MOTOR_LEFT_LEG,3,3000,ACCEL_HIP,X,0,-1,1,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=5|MAX_REVERSE_THRUST=3)
5,MotorAccel(MOTOR_RIGHT_LEG,3,3000,ACCEL_HIP,X,0,-1,1,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=5|MAX_REVERSE_THRUST=3)
#5,MotorAccel(MOTOR_LEFT_LEG,3,3000,ACCEL_TORSO,X,-15,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=10|MAX_REVERSE_THRUST=3|OPPOSE_MOTION=0.5:0.8)
#5,MotorAccel(MOTOR_RIGHT_LEG,3,3000,ACCEL_TORSO,X,-15,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=10|MAX_REVERSE_THRUST=3|OPPOSE_MOTION=0.5:0.8)
#now RKM just opposes with bias against pos motion, was: RKM needs to push negative upper right leg from around 20 to 15 (was CurrentAngle)
#5,MotorAccel(MOTOR_RIGHT_KNEE,9,3000,ACCEL_RIGHT_UPPER_LEG,X,15,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=6|MAX_VELOCITY=20|ABORT_ANGLE0=40|OPPOSE_MOTION=0.25:1.0)
#5,MotorAccel(MOTOR_RIGHT_KNEE,9,3000,ACCEL_TORSO,Z,0,-2,2,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=6|MAX_VELOCITY=20|ABORT_ANGLE0=40)
5,MotorAccel(MOTOR_RIGHT_KNEE,16,3000,ACCEL_TORSO,Z,0,-2,2,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=3|MAX_VELOCITY=20|ABORT_ANGLE0=40)  
#5,MotorAccel(MOTOR_LEFT_KNEE,9,3000,ACCEL_LEFT_UPPER_LEG,X,CurrentAngle,-1,1,WAIT_STAGE|CONSTANT_THRUST|HOLD_ANGLE)
#LKM can be used to shift robot Z back to left quickly for now hold at 6   (LULX0 is around 6)
#LKM opposes motion
#5,MotorAccel(MOTOR_LEFT_KNEE,16,3000,ACCEL_LEFT_UPPER_LEG,X,6,-1,1,WAIT_STAGE|HOLD_ANGLE=1.0:0.5|MAX_THRUST=6|MAX_VELOCITY=20)
#5,MotorAccel(MOTOR_LEFT_KNEE,16,3000,ACCEL_LEFT_LOWER_LEG,X,-20,-1,1,WAIT_STAGE|HOLD_ANGLE=1.0:0.5|MAX_THRUST=6|MAX_VELOCITY=20)
#5,MotorAccel(MOTOR_LEFT_KNEE,16,3000,ACCEL_TORSO,Z,0,-2,2,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=6|MAX_VELOCITY=20)
#5,MotorAccel(MOTOR_LEFT_KNEE,16,3000,ACCEL_TORSO,Z,0,-2,2,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=8|MAX_VELOCITY=20)
#to stop LUL from going past 0 deg, strive for X=2 deg (LULX0=6)- that also helps the TORSO X
#5,MotorAccel(MOTOR_LEFT_KNEE,16,3000,ACCEL_LEFT_UPPER_LEG,X,LULX0,-2,2,ACCEL_TORSO,Z,0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=3|MAX_VELOCITY=20)
5,MotorAccel(MOTOR_LEFT_KNEE,16,3000,ACCEL_LEFT_UPPER_LEG,X,LULX0,-1,1,ACCEL_HIP,Z,0,-1,1,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST0=8|MAX_REVERSE_THRUST0=8|MAX_THRUST1=3|MAX_REVERSE_THRUST1=3|MAX_VELOCITY0=20|MAX_VELOCITY1=10)
#left ankle needs to go back to the original angle - based on left lower leg Z 3deg
#bias LAM to pos
#5,MotorAccel(MOTOR_LEFT_ANKLE,3,3000,ACCEL_LEFT_FOOT,Z,0,-10,10,ACCEL_LEFT_LOWER_LEG,Z,LLLZ0,-3,3,WAIT_STAGE|HOLD_ANGLE=0.5:1|MAX_THRUST=20|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20|MIN_VELOCITY=5|FUTURE_LOG|ABORT_ANGLE0=30)
#5,MotorAccel(MOTOR_LEFT_ANKLE,3,3000,ACCEL_LEFT_FOOT,Z,0,-10,10,ACCEL_LEFT_UPPER_LEG,Z,LULZ0,-3,3,WAIT_STAGE|HOLD_ANGLE=0.5:1|MAX_THRUST=20|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20|MIN_VELOCITY=5|FUTURE_LOG|ABORT_ANGLE0=30)
5,MotorAccel(MOTOR_LEFT_ANKLE,3,3000,ACCEL_LEFT_FOOT,Z,0,-10,10,ACCEL_LEFT_UPPER_LEG,Z,LULZ0,-3,3,WAIT_STAGE|HOLD_ANGLE=0.5:1|MAX_THRUST=20|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20|MIN_VELOCITY=5|FUTURE_LOG|ABORT_ANGLE0=30)
#bias RAM to neg
#5,MotorAccel(MOTOR_RIGHT_ANKLE,3,3000,ACCEL_RIGHT_FOOT,Z,0,-10,10,ACCEL_RIGHT_LOWER_LEG,Z,RLLZ0,-3,3,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=20|MAX_REVERSE_THRUST=3|ABORT_ANGLE0=30)
#5,MotorAccel(MOTOR_RIGHT_ANKLE,3,3000,ACCEL_RIGHT_FOOT,Z,0,-10,10,ACCEL_RIGHT_UPPER_LEG,Z,RULZ0,-3,3,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=20|MAX_REVERSE_THRUST=3|ABORT_ANGLE0=30)
5,MotorAccel(MOTOR_RIGHT_ANKLE,3,3000,ACCEL_RIGHT_FOOT,Z,0,-10,10,ACCEL_RIGHT_UPPER_LEG,Z,RULZ0,-3,3,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=20|MAX_REVERSE_THRUST=3|ABORT_ANGLE0=30)
#feet hold lower legs to initial angle 
#MAX_THRUST on left foot motor can now increase to push body down onto right foot
#left foot needs to tilt body forward  (LLLX0 is around -12)
#5,MotorAccel(MOTOR_LEFT_FOOT,5,3000,ACCEL_LEFT_FOOT,X,0,-2,2,ACCEL_LEFT_LOWER_LEG,X,LLLX0,-1,1,WAIT_STAGE|HOLD_ANGLE|FUTURE_LOG|MAX_THRUST=8)
#LFM bring LLX from -12/-15 to -20
#5,MotorAccel(MOTOR_LEFT_FOOT,16,3000,ACCEL_LEFT_FOOT,X,0,-4,4,ACCEL_LEFT_LOWER_LEG,X,-20,-1,1,WAIT_STAGE|HOLD_ANGLE|FUTURE_LOG|MAX_THRUST=14)
#LFM seek to stop all motion
#5,MotorAccel(MOTOR_LEFT_FOOT,16,3000,ACCEL_LEFT_FOOT,X,0,-10,10,ACCEL_LEFT_LOWER_LEG,X,-20,-1,1,WAIT_STAGE|HOLD_ANGLE|FUTURE_LOG|MAX_THRUST=8|OPPOSE_MOTION=0.5:0.8)
#5,MotorAccel(MOTOR_LEFT_FOOT,10,3000,ACCEL_LEFT_FOOT,X,0,-10,10,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE=2:2|MAX_THRUST=16|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20)
#5,MotorAccel(MOTOR_LEFT_FOOT,10,3000,ACCEL_LEFT_LOWER_LEG,X,LLLX0,-2,2,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE=2:2|MAX_THRUST=16|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20)
5,MotorAccel(MOTOR_LEFT_FOOT,10,3000,ACCEL_LEFT_FOOT,X,0,-5,5,ACCEL_LEFT_LOWER_LEG,X,LLLX0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.5:2|MAX_THRUST=16|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20)
#5,MotorAccel(MOTOR_LEFT_FOOT,16,3000,ACCEL_LEFT_LOWER_LEG,X,-20,-1,1,WAIT_STAGE|HOLD_ANGLE|FUTURE_LOG|MAX_THRUST=14|OPPOSE_MOTION=0.7:0.4)
#right foot needs to bring RFX to 0
#5,MotorAccel(MOTOR_RIGHT_FOOT,16,3000,ACCEL_RIGHT_FOOT,X,0,-4,4,ACCEL_RIGHT_LOWER_LEG,X,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=14)
#RFM oppose all motion
#5,MotorAccel(MOTOR_RIGHT_FOOT,16,3000,ACCEL_RIGHT_FOOT,X,0,-10,10,ACCEL_RIGHT_LOWER_LEG,X,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=8|OPPOSE_MOTION=0.5:0.8)
#5,MotorAccel(MOTOR_RIGHT_FOOT,10,3000,ACCEL_RIGHT_FOOT,X,0,-10,10,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE=2:2|MAX_THRUST=16|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20)
#5,MotorAccel(MOTOR_RIGHT_FOOT,10,3000,ACCEL_RIGHT_LOWER_LEG,X,RLLX0,-2,2,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.5:2|MAX_THRUST=16|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20)
5,MotorAccel(MOTOR_RIGHT_FOOT,10,3000,ACCEL_RIGHT_FOOT,X,0,-5,5,ACCEL_RIGHT_LOWER_LEG,X,RLLX0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.5:2|MAX_THRUST=16|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20)
#5,MotorAccel(MOTOR_RIGHT_FOOT,16,3000,ACCEL_RIGHT_LOWER_LEG,X,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=14|OPPOSE_MOTION=0.7:0.4)
#torso oppose motion
#maxthrust was 10- needs something better to avoid falling over backwards if angle >-5
5,MotorAccel(MOTOR_TORSO,3,3000,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.5:5|MAX_THRUST=6|MAX_REVERSE_THRUST=2|MAX_VELOCITY=20|FUTURE_LOG)

#possibly right leg motor needs to hold up leg or possibly falls down

10000,StopLogging

