#RightStepForward05.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
##both ankles lean ankle until LLLZ is 0, thrust=7
##alt: left ankle turns until LLLZ=-1 LLLZ starts at 3, important to limit thrust
##possibly 2 motors is the reason stage 1 lasts for a few ms after the target range is reached
##1,MotorAccel(MOTOR_LEFT_SIDE,MOTOR_RIGHT_SIDE,3,5000,ACCEL_LEFT_LOWER_LEG,Z,-4,-2,1,WAIT_STAGE|DOUBLE_THRUST|CONSTANT_THRUST)
#maxv=12 minv=5
#Most of the time there is remaining velocity when reaching the target (which should be around 7, so make target 6.5-8.5)
1,MotorAccel(MOTOR_LEFT_SIDE,3,5000,ACCEL_LEFT_LOWER_LEG,Z,-7,-1,0.5,WAIT_STAGE|DOUBLE_THRUST|MAX_VELOCITY=10|MIN_VELOCITY=5|MAX_THRUST=3|MAX_REVERSE_THRUST=10)
1,MotorAccel(MOTOR_RIGHT_SIDE,3,5000,ACCEL_LEFT_LOWER_LEG,Z,-7,-1,0.5,WAIT_STAGE|HOLD_ANGLE|DOUBLE_THRUST|MAX_VELOCITY=10|MIN_VELOCITY=5|MAX_THRUST=3|MAX_REVERSE_THRUST=10)
#maxv=10 minv=5
1,MotorAccel(MOTOR_LEFT_ANKLE,2,5000,ACCEL_LEFT_FOOT,Z,0,-10,5,ACCEL_LEFT_LOWER_LEG,Z,-8,-1,0.5,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=2|MAX_REVERSE_THRUST=6|MAX_VELOCITY=9|MIN_VELOCITY=7|FUTURE_LOG|MAINTAIN_THRUST)
1,MotorAccel(MOTOR_RIGHT_ANKLE,2,5000,ACCEL_RIGHT_FOOT,Z,0,-10,5,ACCEL_LEFT_LOWER_LEG,Z,-8,-1,0.5,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=2|MAX_REVERSE_THRUST=6|MAX_VELOCITY=9|MIN_VELOCITY=7)
##feet, knee, leg, and torso motors keep torso from falling forward or backward
##torso holds torso x until stage 4- note LAST_STAGE=4 needs 10s timeout, torso should not be using a lot of thrust, only small adjustments
##1,MotorAccel(MOTOR_TORSO,3,10000,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=4|MAX_REVERSE_THRUST=3|LAST_STAGE=4|MAX_VELOCITY=20)
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|
2,MotorAccel(MOTOR_LEFT_SIDE,7,4000,ACCEL_LEFT_LOWER_LEG,Z,-7.5,-2,2,ACCEL_TORSO,Z,-5,-5,3,WAIT_STAGE|MAX_THRUST=12|ABORT_ON_TIMEOUT|MAX_VELOCITY=30|MIN_VELOCITY=10|FUTURE_LOG)
#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)
#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|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|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=4|MAX_REVERSE_THRUST=3|MAINTAIN_THRUST|MAX_VELOCITY=20|OPPOSE_MOTION=0.5:0.25)


#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
3,MotorAccel(MOTOR_RIGHT_LEG,16,600,ACCEL_RIGHT_UPPER_LEG,X,20,-1,5,WAIT_STAGE|CONSTANT_THRUST)
3,MotorAccel(MOTOR_LEFT_LEG,16,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,12,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
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)
#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,-8,-2,0,WAIT_STAGE|HOLD_ANGLE=10.0:1.0|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MAX_VELOCITY=5|MAINTAIN_THRUST|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,-12,-1,0,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)
#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,-12,-1,0,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MAX_VELOCITY=10|MAINTAIN_THRUST|OPPOSE_MOTION_MAINTAIN=4.0:0.0)
#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,-2,2,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE=2:0.5|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)
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.
3,MotorAccel(MOTOR_RIGHT_FOOT,10,600,ACCEL_RIGHT_FOOT,X,0,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=12|MAX_REVERSE_THRUST=3|MAX_VELOCITY=5)


#Stage 4: LOWER RIGHT LEG SWINGS FORWARD, left side holds right leg off the ground 
#right knee swings forward 
4,MotorAccel(MOTOR_RIGHT_KNEE,12,230,ACCEL_RIGHT_LOWER_LEG,X,0,-1,1,WAIT_STAGE|CONSTANT_THRUST|ABORT_ANGLE0=-30)
#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,230,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)
#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)
#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,230,ACCEL_LEFT_LOWER_LEG,Z,LLLZ0,-1,1,WAIT_STAGE|HOLD_ANGLE=1:1|MAX_THRUST=20|MAX_REVERSE_THRUST=2|ABORT_ANGLE0=20|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,-12,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MAINTAIN_THRUST|OPPOSE_MOTION_MAINTAIN=4.0:0.0|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,-12,-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,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,230,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)
#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,16,230,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,16,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,230,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)
#|ABORT_ANGLE1=-30  make -2 2 range?


#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=8|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,2,-2,2,ACCEL_TORSO,Z,0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=8|MAX_VELOCITY=20)
#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,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,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.1:10|MAX_THRUST=16|MAX_REVERSE_THRUST=2|MAX_VELOCITY=30)

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

10000,StopLogging

