#Right2StepUpDown02.sh
#for 24rpm feet, ankle, knee, leg, side and torso 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
#not having |NO_STOP_ON_TARGET on side motors does make a difference
#stage 1 requires: Accel Left Leg lower and upper leg z to reach target zone
#-4.0 was -4.5
1,MotorAccel(MOTOR_LEFT_SIDE,3,5000,ACCEL_LEFT_LOWER_LEG,Z,-3.0,-6,1,ACCEL_LEFT_UPPER_LEG,Z,-3.0,-2,1,WAIT_STAGE|MIN_VELOCITY=5|MAX_VELOCITY=7|MAX_THRUST=3|MAX_REVERSE_THRUST=2|MAX_ACCEL=0.1|ABORT_ON_TIMEOUT|REQUIRED)
1,MotorAccel(MOTOR_RIGHT_SIDE,3,5000,ACCEL_LEFT_LOWER_LEG,Z,-3.0,-2,1,ACCEL_LEFT_UPPER_LEG,Z,-3.0,-2,1,WAIT_STAGE|MIN_VELOCITY=5|MAX_VELOCITY=7|MAX_THRUST=3|MAX_REVERSE_THRUST=2|MAX_ACCEL=0.1)
#|MAINTAIN_THRUST
#set ankle motor to target a farther angle, because then it will likely still have some velocity when stage 1 ends 
#1,MotorAccel(MOTOR_LEFT_ANKLE,1,5000,ACCEL_LEFT_FOOT,Z,0,-10,5,ACCEL_LEFT_LOWER_LEG,Z,-6,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=5|MAX_REVERSE_THRUST=5|MIN_VELOCITY=5|MAX_VELOCITY=8|MAX_ACCEL=0.1|FUTURE_LOG|MAINTAIN_THRUST)
#1,MotorAccel(MOTOR_RIGHT_ANKLE,3,5000,ACCEL_RIGHT_FOOT,Z,0,-10,5,ACCEL_LEFT_LOWER_LEG,Z,-6,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=5|MAX_REVERSE_THRUST=5|MIN_VELOCITY=5|MAX_VELOCITY=8|MAX_ACCEL=0.1|MAINTAIN_THRUST)
1,MotorAccel(MOTOR_LEFT_ANKLE,1,5000,ACCEL_LEFT_LOWER_LEG,Z,-4.0,-2,1,ACCEL_LEFT_UPPER_LEG,Z,-4.0,-2,1,WAIT_STAGE|MAX_THRUST=4|MAX_REVERSE_THRUST=3|MIN_VELOCITY=5|MAX_VELOCITY=8|MAX_ACCEL=0.1|FUTURE_LOG|MAINTAIN_THRUST)
1,MotorAccel(MOTOR_RIGHT_ANKLE,3,5000,ACCEL_LEFT_LOWER_LEG,Z,-4.0,-2,1,ACCEL_LEFT_UPPER_LEG,Z,-4.0,-2,1,WAIT_STAGE|MAX_THRUST=4|MAX_REVERSE_THRUST=3|MIN_VELOCITY=5|MAX_VELOCITY=8|MAX_ACCEL=0.1|MAINTAIN_THRUST)
1,MotorAccel(MOTOR_TORSO,1,5000,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|TVR=0.2:0.5:1|MAX_THRUST=6|MAX_REVERSE_THRUST=2|MAINTAIN_THRUST|MAX_VELOCITY=10|MIN_ACCEL=1|FUTURE_LOG)
#NO_STOP_ON_TARGET|


#stage 2: LEFT_SIDE reverses, to make torso Z straighten, and put torso weight over left leg, can be more positive than 1deg
#stage 2 is meant to be a one time pnly fast stage- not a stage that has to "try" more than one time to reach its required angles
#2,MotorAccel(MOTOR_LEFT_SIDE,4,4000,ACCEL_LEFT_LOWER_LEG,Z,-7,-2,0.5,ACCEL_TORSO,Z,-5,-1,1,WAIT_STAGE|MAX_THRUST0=4|MAX_REVERSE_THRUST0=4|MIN_VELOCITY0=5|MAX_VELOCITY0=8|MAX_THRUST1=10|MAX_REVERSE_THRUST1=4|MIN_VELOCITY1=20|MAX_VELOCITY1=40|MAX_ACCEL0=0.1|MAX_ACCEL1=5|FUTURE_LOG|NO_STOP_ON_TARGET|ABORT_ON_TIMEOUT)
#sometimes the LULZ is apparently reaching even nearly -8, so make sure stage can end if that happens
#use HIPZ?
#possibly right side could helkp but only to LULZ=0
#below inst skips attempt at reversing if not in range
#4,-1,0.5
2,MotorAccel(MOTOR_LEFT_SIDE,12,1400,ACCEL_TORSO,Z,-3,-1,0.5,WAIT_STAGE|MAX_THRUST=12|MAX_REVERSE_THRUST=4|MIN_VELOCITY=30|MAX_VELOCITY=50|MAX_ACCEL=5|FUTURE_LOG|NO_STOP_ON_TARGET|ABORT_ON_TIMEOUT|REQUIRED)
##2,MotorAccel(MOTOR_LEFT_SIDE,11,1400,ACCEL_LEFT_UPPER_LEG,Z,-2.5,-6,1,ACCEL_TORSO,Z,-5,-1,1,WAIT_STAGE|MAX_THRUST0=4|MAX_REVERSE_THRUST0=4|MIN_VELOCITY0=5|MAX_VELOCITY0=8|MAX_THRUST1=11|MAX_REVERSE_THRUST1=4|MIN_VELOCITY1=20|MAX_VELOCITY1=50|MAX_ACCEL0=0.1|MAX_ACCEL1=5|FUTURE_LOG|NO_STOP_ON_TARGET|ABORT_ON_TIMEOUT)
#important note: right side motor picks up momentum in stage 2, but needs to reverse for right leg lift (stage 3), 
#it is mysterious but sometimes the LLLZ can have a -V but LULZ has a +V so watch LULZ first
#20 is a little too fast sometimes and turns foot too quickly, changed 20:0.5 to 5:0.5
#for stage 2 LLLZ needs to be before LULZ because LLLZ can bend too fast and the ankle motor needs to reverse
#ACCEL_LEFT_LOWER_LEG,Z,-4.5,-3.5,0.5,
2,MotorAccel(MOTOR_LEFT_ANKLE,3,1400,ACCEL_LEFT_FOOT,Z,0,-10,15,ACCEL_LEFT_UPPER_LEG,Z,-3.5,-0.5,0.5,WAIT_STAGE|MAX_THRUST=19|MAX_REVERSE_THRUST=6|MIN_VELOCITY=10|MAX_VELOCITY=15|ABORT_ANGLE0=30|TVR=15:1:3|MAX_ACCEL=1|NO_STOP_ON_TARGET|MAINTAIN_THRUST|FUTURE_LOG)
#2,MotorAccel(MOTOR_LEFT_ANKLE,3,1400,ACCEL_LEFT_FOOT,Z,0,-10,10,ACCEL_LEFT_LOWER_LEG,Z,-4.5,-3.0,0.5,ACCEL_LEFT_UPPER_LEG,Z,-4.5,-0.5,0.5,WAIT_STAGE|MAX_THRUST=20|MAX_REVERSE_THRUST=6|MIN_VELOCITY=10|MAX_VELOCITY=15|ABORT_ANGLE0=30|HOLD_ANGLE=15:0.8|MAX_ACCEL=1|NO_STOP_ON_TARGET|MAINTAIN_THRUST|FUTURE_LOG)
#2,MotorAccel(MOTOR_RIGHT_ANKLE,4,1400,ACCEL_RIGHT_FOOT,Z,0,-10,5,ACCEL_LEFT_LOWER_LEG,Z,-7.0,-1,0.5,WAIT_STAGE|HOLD_ANGLE=20:5|MAX_THRUST=6|MAX_REVERSE_THRUST=6|MIN_VELOCITY=5|MAX_VELOCITY=8|MAX_ACCEL1=0.1|NO_STOP_ON_TARGET)
2,MotorAccel(MOTOR_RIGHT_ANKLE,4,1400,ACCEL_LEFT_UPPER_LEG,Z,-3,-3,1,ACCEL_LEFT_LOWER_LEG,Z,-4,-3,1,WAIT_STAGE|TVR=20:5:4|MAX_THRUST=6|MAX_REVERSE_THRUST=6|MIN_VELOCITY=5|MAX_VELOCITY=8|MAX_ACCEL1=0.1|NO_STOP_ON_TARGET)
2,MotorAccel(MOTOR_LEFT_KNEE,12,1400,ACCEL_LEFT_UPPER_LEG,X,LULX0,-2,2,WAIT_STAGE|MAX_THRUST=12|MAX_REVERSE_THRUST=6|MAINTAIN_THRUST|ABORT_ANGLE0=25)
#|NO_STOP_ON_TARGET
#feet hold up lower legs
2,MotorAccel(MOTOR_LEFT_FOOT,3,1400,ACCEL_LEFT_LOWER_LEG,X,CurrentAngle,-1,1,WAIT_STAGE|TVR=0.25:0.5:3|MAX_THRUST=7|MAX_REVERSE_THRUST=3|MAX_VELOCITY=10)
2,MotorAccel(MOTOR_RIGHT_FOOT,3,1400,ACCEL_RIGHT_LOWER_LEG,X,CurrentAngle,-1,1,WAIT_STAGE|TVR=0.25:0.5:3|MAX_THRUST=7|MAX_REVERSE_THRUST=3|MAX_VELOCITY=10)
2,MotorAccel(MOTOR_TORSO,3,1400,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|TVR=0.2:0.5:3|MAX_THRUST=7|MAX_REVERSE_THRUST=3|MAINTAIN_THRUST|MAX_VELOCITY=10|MIN_ACCEL=1|FUTURE_LOG)
#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 angle=16
#important: changing these 3 20s to 18s can cause the EthMotors PCB (presumably PIC) to stop communication, presumably from EMF, and noise can be seen in the Touch sensor lines.
#6->15, is now 12->21
3,MotorAccel(MOTOR_RIGHT_LEG,14,400,ACCEL_RIGHT_UPPER_LEG,X,21,-1,5,WAIT_STAGE|CONSTANT_THRUST|REQUIRED)
3,MotorAccel(MOTOR_LEFT_LEG,14,400,ACCEL_RIGHT_UPPER_LEG,X,21,-1,5,WAIT_STAGE|CONSTANT_THRUST)
#bend right knee so foot lifts in place - decrease angle X (lower leg rotates forward)
#14=too much momentum into stage 4
3,MotorAccel(MOTOR_RIGHT_KNEE,20,400,ACCEL_RIGHT_LOWER_LEG,X,-20,-1,1,WAIT_STAGE|CONSTANT_THRUST)
#HOLD_ANGLE=1:0.5
3,MotorAccel(MOTOR_LEFT_KNEE,12,400,ACCEL_LEFT_UPPER_LEG,X,LULX0,-2,2,WAIT_STAGE|MAX_THRUST=12|MAX_REVERSE_THRUST=6|MAINTAIN_THRUST|ABORT_ANGLE0=25)
#TODO: *** MAINTAIN_THRUST is not working here ***
#3,MotorAccel(MOTOR_LEFT_ANKLE,12,400,ACCEL_LEFT_LOWER_LEG,Z,-7,-1,0,WAIT_STAGE|HOLD_ANGLE=20.0:2.0|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MAX_VELOCITY=5|MAINTAIN_THRUST|ABORT_ANGLE0=30|FUTURE_LOG)
#MAX_THRUST for 9rpm was=20
#cannot use FARTHEST_FROM_TARGET| - because can go to foot
#no MAINTAIN_THRUST| - because stage 4 LAM needs to start with initial thrust
#MAX THRUST was 20 but Left Ankle is lifting off ground, set FOOTZ max to 8
3,MotorAccel(MOTOR_LEFT_ANKLE,15,400,ACCEL_LEFT_FOOT,Z,0,-10,8,ACCEL_LEFT_LOWER_LEG,Z,-7,-0.5,0.5,ACCEL_LEFT_UPPER_LEG,Z,-4,-0.5,0.5,WAIT_STAGE|TVR=20.0:3.0|MAX_THRUST=15|MAX_REVERSE_THRUST=7|MIN_VELOCITY=5|MAX_VELOCITY=20|MAX_ACCEL=0.5|ABORT_ANGLE0=30|OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET|FUTURE_LOG)
#3,MotorAccel(MOTOR_LEFT_ANKLE,12,400,ACCEL_LEFT_FOOT,Z,0,-10,20,ACCEL_LEFT_LOWER_LEG,Z,-8,-1,1,WAIT_STAGE|HOLD_ANGLE=20.0:2.0|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MAX_VELOCITY=5|MAX_ACCEL=0.5|MAINTAIN_THRUST|CONSTANT_THRUST=2:20|ABORT_ANGLE0=30|FUTURE_LOG)
#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,400,ACCEL_RIGHT_FOOT,Z,0,-10,5,ACCEL_LEFT_LOWER_LEG,Z,-7,-2,1,WAIT_STAGE|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MAX_VELOCITY=5|MAX_ACCEL=0.5)
#3,MotorAccel(MOTOR_LEFT_SIDE,20,400,ACCEL_TORSO,Z,-8,-1,1,ACCEL_LEFT_LOWER_LEG,Z,-13,-1,1,WAIT_STAGE|HOLD_ANGLE=40:0|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MIN_VELOCITY=5|MAX_VELOCITY=30|MAINTAIN_THRUST|NO_STOP_ON_TARGET|FUTURE_LOG)
#pretty much there is not a lot of way to save the ankle one the ankle starts falling
#ACCEL_LEFT_LOWER_LEG,Z,-6,-1,6,
#it seems likely that RSM should be no higher MAX_THRUST than LSM in stages 3 and 4 because otherwise it seems that the robot falls to the right as the RSM is using more thrust than the LSM torso was -8
#tough decision between watching RULZ or TORSOZ
#do not use MAINTAIN_THRUST| because there is no MOTOR_LEFT_SIDE inst in stage 4
#LSM needs a lot of thrust to hold torso z left against weight of lifted right leg
#LSM always needs to be thrusting, so set the target too far, and the thrust too little
3,MotorAccel(MOTOR_LEFT_SIDE,19,400,ACCEL_TORSO,Z,-8,-1,1,WAIT_STAGE|TVR=1:0:8|MAX_THRUST=19|MAX_REVERSE_THRUST=2|MIN_VELOCITY=20|MAX_VELOCITY=50|MAX_ACCEL=5.0|NO_STOP_ON_TARGET|MAINTAIN_THRUST|FUTURE_LOG)
#3,MotorAccel(MOTOR_LEFT_SIDE,16,400,ACCEL_TORSO,Z,-6,-1,1,ACCEL_RIGHT_UPPER_LEG,Z,-7,-1,1,ACCEL_LEFT_UPPER_LEG,Z,0,0,0,WAIT_STAGE|HOLD_ANGLE=40:0|MAX_THRUST=16|MAX_REVERSE_THRUST=2|MIN_VELOCITY=20|MAX_VELOCITY=50|MAX_ACCEL=5.0|NO_STOP_ON_TARGET|RELATIVE_ANGLE1,FUTURE_LOG)
#3,MotorAccel(MOTOR_RIGHT_SIDE,13,400,ACCEL_RIGHT_UPPER_LEG,Z,-10,-1,1,ACCEL_LEFT_UPPER_LEG,Z,0,0,0,WAIT_STAGE|HOLD_ANGLE=20:0|MAX_THRUST=13|MAX_REVERSE_THRUST=2|MIN_VELOCITY=5|MAX_VELOCITY=40|MAX_ACCEL=3.0|NO_STOP_ON_TARGET|RELATIVE_ANGLE0|MAINTAIN_THRUST)
#maxthrust=13
#4/8/18 because RSM and RLM are both needed to lift RL (RLM cannot lift RL without RSM lifting RL too), added RSM RULX association. Use high accel and velocity because thrust should be constant. RSM needs to monitor leg separation in addition to help to bring RULX up too
3,MotorAccel(MOTOR_RIGHT_SIDE,19,400,ACCEL_RIGHT_UPPER_LEG,Z,-7,-1,1,ACCEL_LEFT_UPPER_LEG,Z,0,0,0,ACCEL_RIGHT_UPPER_LEG,X,21,-1,5,WAIT_STAGE|TVR=20:0:8|MAX_THRUST=19|MAX_REVERSE_THRUST=2|MIN_VELOCITY=80|MAX_VELOCITY=100|MAX_ACCEL=10.0|NO_STOP_ON_TARGET|RELATIVE_ANGLE0|MAINTAIN_THRUST)
#3,MotorAccel(MOTOR_RIGHT_SIDE,14,400,ACCEL_RIGHT_UPPER_LEG,Z,-7,-1,1,ACCEL_LEFT_UPPER_LEG,Z,0,0,0,WAIT_STAGE|HOLD_ANGLE=20:0|MAX_THRUST=14|MAX_REVERSE_THRUST=2|MIN_VELOCITY=15|MAX_VELOCITY=40|MAX_ACCEL=3.0|NO_STOP_ON_TARGET|RELATIVE_ANGLE0|MAINTAIN_THRUST)
#send LLL forward slightly to stop fall backwards -12 to -14
#HOLD_ANGLE=2:2
3,MotorAccel(MOTOR_LEFT_FOOT,5,400,ACCEL_LEFT_FOOT,X,0,-10,10,ACCEL_LEFT_LOWER_LEG,X,LLLX0,-1,1,WAIT_STAGE|ABORT_ANGLE1=-30|MAX_THRUST=16|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20)
#3,MotorAccel(MOTOR_LEFT_FOOT,5,400,ACCEL_LEFT_FOOT,X,0,-10,10,ACCEL_LEFT_LOWER_LEG,X,-14,-1,1,WAIT_STAGE|HOLD_ANGLE=2:2|ABORT_ANGLE1=-30|MAX_THRUST=16|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20)
#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,400,ACCEL_RIGHT_FOOT,X,-2,-5,5,WAIT_STAGE|MAX_THRUST=12|MAX_REVERSE_THRUST=3|MAX_VELOCITY=5)
#use low TVR because MaxThrust=4 and <20 degrees not a lot of thrust is needed
#there is a large force pushing the torso back
#=1:1, MIN_VELOCITY needs to be somewhat higher because otherwise V reverses too much
3,MotorAccel(MOTOR_TORSO,3,400,ACCEL_TORSO,X,TX0,-1,1,WAIT_STAGE|TVR=0.2:0.4:3|MAX_THRUST=6|MAX_REVERSE_THRUST=3|MIN_VELOCITY=5|MAX_VELOCITY=10|MAX_ACCEL=1|NO_MOTION_NO_THRUST|OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET|MAINTAIN_THRUST)


#Stage 4: LOWER RIGHT LEG SWINGS BACK, UPPER LEG COMES BACK DOWN, left side holds right leg off the ground, ends when right foot touches ground (this is so right ankle motor can know that the foot is on the ground to look at the RULZ angle)
#***note no HOLD_ANGLE***
#end stage if Right foot front or back touch sensors > 3
#required to end stage 4: RULX reaches RULX0 +-1 or Right foot front or back Touch Sensor > 3
4,TouchSensor(TOUCH_RIGHT_FOOT_FRONT,GT,50.0,250,800)
4,TouchSensor(TOUCH_RIGHT_FOOT_BACK,GT,50.0,250,800)
#4,MotorAccel(MOTOR_RIGHT_LEG,20,400,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-1,1,WAIT_STAGE|CONSTANT_THRUST)
#left ankle controls how fast weight changes from left side to right side, so much watch the HIP Z
#HOLD_ANGLE=4:2|
#4,MotorAccel(MOTOR_LEFT_ANKLE,3,400,ACCEL_LEFT_FOOT,Z,0,-5,5,ACCEL_HIP,Z,3,-1,1,WAIT_STAGE|MAX_THRUST=10|MAX_REVERSE_THRUST=2|MIN_VELOCITY=20|MAX_VELOCITY=40|MAX_ACCEL=5|FUTURE_LOG|ABORT_ANGLE1=30|MAINTAIN_THRUST)
#LAM inst is pretty critical in this stage
#stage 4 should not end until the HIP Z is 2-4, and the LUL Z is around 3. MIN_VELOCITY is important- controls speed in fall to right
#a problem arises when V=Y
#|CONSTANT_THRUST=14 - really needs to meet a minimum velocity of perhaps 40dps? hip is 3,-1,5 just to make sure stage 4 does not end before rt foot touches ground 
#MAX_REVERSE_THRUST should be high to slow down fall if LULZ=+  (accel was 10)
4,MotorAccel(MOTOR_LEFT_ANKLE,18,800,ACCEL_LEFT_FOOT,Z,0,-10,15,ACCEL_LEFT_UPPER_LEG,Z,2,-1,10,ACCEL_LEFT_LOWER_LEG,Z,3,-1,10,ACCEL_HIP,Z,1,-1,9,WAIT_STAGE|MAX_THRUST=18|MAX_REVERSE_THRUST=18|MIN_VELOCITY=25|MAX_VELOCITY=45|MAX_ACCEL=5|FUTURE_LOG|ABORT_ANGLE2=30|NO_STOP_ON_TARGET|MAINTAIN_THRUST|ABORT_ON_TIMEOUT|REQUIRED)
#4,MotorAccel(MOTOR_LEFT_ANKLE,18,800,ACCEL_LEFT_FOOT,Z,0,-10,15,ACCEL_LEFT_UPPER_LEG,Z,2,-1,10,ACCEL_LEFT_LOWER_LEG,Z,3,-1,10,ACCEL_HIP,Z,1,-1,9,WAIT_STAGE|MAX_THRUST=18|MAX_REVERSE_THRUST=10|MIN_VELOCITY=25|MAX_VELOCITY=45|MAX_ACCEL=10|FUTURE_LOG|ABORT_ANGLE2=30|NO_STOP_ON_TARGET|MAINTAIN_THRUST|ABORT_ON_TIMEOUT)
#4,MotorAccel(MOTOR_LEFT_ANKLE,18,800,ACCEL_LEFT_FOOT,Z,0,-10,15,ACCEL_LEFT_UPPER_LEG,Z,2,-1,10,ACCEL_LEFT_LOWER_LEG,Z,2,-1,10,ACCEL_HIP,Z,3,-1,5,WAIT_STAGE|MAX_THRUST=18|MAX_REVERSE_THRUST=10|MIN_VELOCITY=20|MAX_VELOCITY=40|MAX_ACCEL=10|FUTURE_LOG|ABORT_ANGLE2=30|NO_STOP_ON_TARGET|MAINTAIN_THRUST|ABORT_ON_TIMEOUT)
#leg motors bring right leg down
#stage 3 Thrust=12 (against gravity) so stage 4 Thrust should be less
4,MotorAccel(MOTOR_RIGHT_LEG,8,800,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-2,2,WAIT_STAGE|CONSTANT_THRUST)
4,MotorAccel(MOTOR_LEFT_LEG,8,800,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-2,2,WAIT_STAGE|MAX_THRUST=18|MAX_REVERSE_THRUST=3|CONSTANT_THRUST)
#right knee swings forward - needs HOLD_ANGLE upper leg motor can stop stage
#target angle was RLLX0 (-12), but needs to be relative angle so RLLX=RULX-24 so when RULX=21 LULX will be -3 and work down to -12
#4,MotorAccel(MOTOR_RIGHT_KNEE,17,700,ACCEL_RIGHT_LOWER_LEG,X,3,0,0,WAIT_STAGE|CONSTANT_THRUST=20:5|HOLD_ANGLE|ABORT_ANGLE0=-30)
4,MotorAccel(MOTOR_RIGHT_KNEE,10,800,ACCEL_RIGHT_LOWER_LEG,X,-24,-1,1,ACCEL_RIGHT_UPPER_LEG,X,0,0,0,WAIT_STAGE|RELATIVE_ANGLE0|ABORT_ANGLE0=-40|MAX_ACCEL=10|MIN_VELOCITY=30|MAX_VELOCITY=50|MAX_THRUST=19|CONSTANT_THRUST=19:4)
#4,MotorAccel(MOTOR_LEFT_ANKLE,3,800,ACCEL_LEFT_FOOT,Z,0,-10,20,ACCEL_LEFT_LOWER_LEG,Z,-7,-1,1,WAIT_STAGE|HOLD_ANGLE=10:2|MAX_THRUST=20|MAX_REVERSE_THRUST=2|ABORT_ANGLE0=40|MAINTAIN_THRUST)
#flatten left foot
#4,MotorAccel(MOTOR_LEFT_ANKLE,3,800,ACCEL_LEFT_FOOT,Z,0,-1,1,ACCEL_LEFT_LOWER_LEG,Z,LLLZ0,-1,1,WAIT_STAGE|HOLD_ANGLE=10:2|MAX_THRUST=20|MAX_REVERSE_THRUST=2|ABORT_ANGLE0=40)
#Left Ankle needs to look at upper leg too, to lean right - do not care beyond LLLZ0 in the plus direction +10- otherwise foot can turn too far
#hold angle was 10:2
#4,MotorAccel(MOTOR_LEFT_ANKLE,3,800,ACCEL_LEFT_UPPER_LEG,Z,LULZ0,-1,10,ACCEL_LEFT_LOWER_LEG,Z,LLLZ0,-1,10,WAIT_STAGE|HOLD_ANGLE=10:2|MAX_THRUST=20|MAX_REVERSE_THRUST=2|FUTURE_LOG|ABORT_ANGLE0=40)
#MAX_VELOCITY can determine how fast the left side leans right
#4,MotorAccel(MOTOR_LEFT_ANKLE,3,800,ACCEL_LEFT_FOOT,Z,0,-5,5,ACCEL_LEFT_LOWER_LEG,Z,LLLZ0,-1,10,ACCEL_LEFT_UPPER_LEG,Z,LULZ0,-1,10,WAIT_STAGE|HOLD_ANGLE=4:2|MAX_THRUST=10|MAX_REVERSE_THRUST=2|MIN_VELOCITY=20|MAX_VELOCITY=40|MAX_ACCEL=5|FUTURE_LOG|ABORT_ANGLE1=40)
#try a controlled lean back to the right, will need to be biased against gravity and the above weight of the robot
#4,MotorAccel(MOTOR_LEFT_ANKLE,3,800,ACCEL_LEFT_FOOT,Z,0,-10,20,ACCEL_LEFT_LOWER_LEG,Z,LLLZ0,-1,1,WAIT_STAGE|HOLD_ANGLE=10:2|MAX_THRUST=20|MAX_REVERSE_THRUST=2|ABORT_ANGLE0=40|MIN_VELOCITY1=2|MAX_VELOCITY1=5|MAX_ACCEL1=0.1|MAINTAIN_THRUST)
#try RELATIVE_ANGLE0, +6 (LULZ0=3 and RULZ0=-3)
#I could swing the torso to the right in this stage, but it might be a little early
#LSM needs to hold TORSO Z centered- aligned with left leg z (RELATIVE_ANGLE0 = 0 offset), and should not ever thrust minus
#LSM always needs to be thrusting, so set the target too far, and the thrust too little, and MAX_ACCEL high- to stop anything that would stop thrust from being applied
#z leg spacing is most important in stage 4 (but trying to keep torso Z to the left helps stage 5 to lean right, and serves as a counter for the RSM thrust in trying to keep legs apart anyway)
4,MotorAccel(MOTOR_LEFT_SIDE,19,800,ACCEL_TORSO,Z,-8,-1,1,ACCEL_LEFT_UPPER_LEG,Z,7,-1,1,ACCEL_RIGHT_UPPER_LEG,Z,0,0,0,WAIT_STAGE|TVR=1:1:8|MAX_THRUST=19|MAX_REVERSE_THRUST=2|MAX_ACCEL=5|MIN_VELOCITY=10|MAX_VELOCITY=20|RELATIVE_ANGLE1)
#4,MotorAccel(MOTOR_LEFT_SIDE,6,800,ACCEL_LEFT_UPPER_LEG,Z,7,-1,1,ACCEL_RIGHT_UPPER_LEG,Z,0,0,0,WAIT_STAGE|HOLD_ANGLE=1:1|MAX_THRUST=7|MAX_REVERSE_THRUST=2|MAX_ACCEL=1|MIN_VELOCITY=10|MAX_VELOCITY=20|RELATIVE_ANGLE0)
#torso Z follows upper leg Z:
#4,MotorAccel(MOTOR_LEFT_SIDE,6,800,ACCEL_TORSO,Z,5,-1,1,ACCEL_LEFT_UPPER_LEG,Z,0,0,0,WAIT_STAGE|HOLD_ANGLE=5:0|MAX_THRUST=7|MAX_REVERSE_THRUST=2|MAX_ACCEL=1|MIN_VELOCITY=10|MAX_VELOCITY=20|RELATIVE_ANGLE0)
#4,MotorAccel(MOTOR_LEFT_SIDE,6,800,ACCEL_LEFT_UPPER_LEG,Z,7,-1,1,ACCEL_RIGHT_UPPER_LEG,Z,0,0,0,WAIT_STAGE|HOLD_ANGLE=10:0|MAX_THRUST=10|MAX_REVERSE_THRUST=2|RELATIVE_ANGLE0)
#use Torso z as judge of if more active lean right is needed
#left knee may need to lift up the LULX here as oppose to help the HIPX (in the opposite direction)- probably TM needs to be better
#ACCEL_LEFT_UPPER_LEG,Z,7,-2,3,ACCEL_RIGHT_UPPER_LEG,Z,0,0,0,  RELATIVE_ANGLE1|
#removed LSM from stage 4 because can cause an issue with the LAM leaning the body right
#put back because helps put torso weight to right side
#Left side needs to move torso to right side
#for now just let torso Z fall by motion of LAM
##4,MotorAccel(MOTOR_LEFT_SIDE,6,800,ACCEL_TORSO,Z,0,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=7|MAX_REVERSE_THRUST=4|MAX_ACCEL=5|MIN_VELOCITY=30|MAX_VELOCITY=50|FUTURE_LOG)
#was -12, -3 is RLLZ0, -6 is correct relative angle, but -8 will send a little bit fartherf
#4,MotorAccel(MOTOR_RIGHT_SIDE,10,800,ACCEL_RIGHT_UPPER_LEG,Z,-6,-1,1,ACCEL_LEFT_UPPER_LEG,Z,0,0,0,WAIT_STAGE|HOLD_ANGLE=10:2|MAX_THRUST=10|MAX_REVERSE_THRUST=2|MAX_ACCEL=1|RELATIVE_ANGLE0)
#maxthrust was 10
#RULZ should be LULZ-7, and then RLLZ should=RULZ
#RSM should have no maintain_thrust here because RSM is disabled in stage 5
#removed ACCEL_RIGHT_LOWER_LEG,Z,0,-1,1,ACCEL_RIGHT_UPPER_LEG,Z,0,0,0, because stage 4 presumes right leg is in the air
#put back in because, if first angle condition is met, but lower leg trails behind, RSM will continue to thrust to move the lower leg 
#RSM needs to monitor leg separation in addition to help to bring RULX down too. RSM needs to watch TZ because in pushing RULX- so just keep TORSO between -5 and +5
4,MotorAccel(MOTOR_RIGHT_SIDE,3,800,ACCEL_TORSO,Z,0,-5,5,ACCEL_RIGHT_UPPER_LEG,Z,-7,-1,1,ACCEL_LEFT_UPPER_LEG,Z,0,0,0,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-2,2,WAIT_STAGE|TVR=10:1:3|MAX_THRUST0=6|MAX_THRUST1=19|MAX_THRUST2=19|MAX_REVERSE_THRUST=2|MIN_VELOCITY=10|MAX_VELOCITY=30|MAX_ACCEL=3|RELATIVE_ANGLE1|REQUIRED)
#4,MotorAccel(MOTOR_RIGHT_SIDE,18,800,ACCEL_RIGHT_UPPER_LEG,Z,-7,-1,1,ACCEL_LEFT_UPPER_LEG,Z,0,0,0,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-2,2,WAIT_STAGE|HOLD_ANGLE=10:1|MAX_THRUST=18|MAX_REVERSE_THRUST=2|MIN_VELOCITY=10|MAX_VELOCITY=30|MAX_ACCEL=3|RELATIVE_ANGLE0)
#4,MotorAccel(MOTOR_RIGHT_SIDE,8,800,ACCEL_RIGHT_UPPER_LEG,Z,-7,-1,1,ACCEL_LEFT_UPPER_LEG,Z,0,0,0,ACCEL_RIGHT_LOWER_LEG,Z,0,-1,1,ACCEL_RIGHT_UPPER_LEG,Z,0,0,0,WAIT_STAGE|HOLD_ANGLE=10:1|MAX_THRUST=8|MAX_REVERSE_THRUST=2|MIN_VELOCITY=10|MAX_VELOCITY=30|MAX_ACCEL=3|RELATIVE_ANGLE0|RELATIVE_ANGLE2)
#4,MotorAccel(MOTOR_RIGHT_SIDE,16,800,ACCEL_RIGHT_UPPER_LEG,Z,-7,-1,1,ACCEL_LEFT_UPPER_LEG,Z,0,0,0,WAIT_STAGE|HOLD_ANGLE=10:1|MAX_THRUST=16|MAX_REVERSE_THRUST=2|MIN_VELOCITY=10|MAX_VELOCITY=30|MAX_ACCEL=3|RELATIVE_ANGLE0)
#|CONSTANT_THRUST=20:4 NO_STOP_ON_TARGET|
#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|MAX_THRUST=10|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20)
#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)
#I think left knee needs to look primarily at the relative angle between LLLX and LULX
4,MotorAccel(MOTOR_LEFT_KNEE,10,800,ACCEL_LEFT_UPPER_LEG,X,-24,-2,2,ACCEL_LEFT_LOWER_LEG,X,0,0,0,WAIT_STAGE|TVR=1:0.5:5|MAX_THRUST=12|MAX_REVERSE_THRUST=6|ABORT_ANGLE0=30|RELATIVE_ANGLE0)
#4,MotorAccel(MOTOR_LEFT_KNEE,10,800,ACCEL_HIP,X,0,-2,2,ACCEL_HIP,Z,0,-2,2,WAIT_STAGE|TVR=1:0.5:5|MAX_THRUST=12|MAX_REVERSE_THRUST=6|ABORT_ANGLE0=30)
#right ankle and right foot motors should straighten right foot - add a little -2 so right ankle does not stop at 0
#have right ankle start to lean the robot right (since now stage 4 goes into the actual lean right)
#RAM shold only be looking at foot, since during stage 4 right leg is presumed to be in the air/off the ground 
#in stage 4 RAM cannot be turning negative, because it is important to stop the lean right in stage 5 by RAM turning positive
4,MotorAccel(MOTOR_RIGHT_ANKLE,10,800,ACCEL_RIGHT_FOOT,Z,0,-10,10,WAIT_STAGE|TVR=1:1:5|MAX_THRUST=7|MAX_REVERSE_THRUST=6|MIN_VELOCITY=10|MAX_VELOCITY=20|MAX_ACCEL=1)
#4,MotorAccel(MOTOR_RIGHT_ANKLE,10,800,ACCEL_RIGHT_FOOT,Z,2,-10,10,ACCEL_RIGHT_UPPER_LEG,Z,-2,-1,5,ACCEL_RIGHT_LOWER_LEG,Z,4,-3,5,WAIT_STAGE|HOLD_ANGLE=1:1|MAX_THRUST=18|MAX_REVERSE_THRUST=6|MIN_VELOCITY=30|MAX_VELOCITY=50|MAX_ACCEL=5|NO_STOP_ON_TARGET)
4,MotorAccel(MOTOR_RIGHT_FOOT,10,800,ACCEL_RIGHT_FOOT,X,0,-2,2,WAIT_STAGE|TVR=0:1:5|MAX_THRUST=9|MAX_REVERSE_THRUST=6)
#still a need to hold torso forward against thrust making torso fall backward
#=1:1
4,MotorAccel(MOTOR_TORSO,3,800,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|TVR=0.4:0.2:3|MAX_THRUST=4|MAX_REVERSE_THRUST=2|MIN_VELOCITY=10|MAX_VELOCITY=15|MAX_ACCEL=1|NO_MOTION_NO_THRUST|OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET|MAINTAIN_THRUST)


#Stage 5: Robot leans right a little farther than stage 4 (similar to stage 1)
#Stage 5 definitely needs:
# 1) Left and Right Knee motors lift upper leg X back up
# 2) Left and Right Leg motors counter by adjusting Hip X
#ankles go to and hold lean right
#ankles maxthrust=7
#quite often RULZ is farther than RLLZ, but not always
#maxrevthrust is 3 above, but here needs more brake
#need to add foot too
#min_vel=30 max_vel=70 RUL Z was 3 with +8
#its important that the foot never leave the ground too much- possibly side touch sensors could help
#one problem is that the Z of the RUL and RLL do not stay rigidly together- not sure if an error with adding Gyro Velocities or 
#actually is happening. But clearly, the Z of the RUL and RLL is not vertical when standing. 
#RLLZ needs to be accel1 before RULZ to correct if the RLLZ>6 but RULZ is still <-2
#NO_STOP_ON_TARGET|
#ACCEL_RIGHT_LOWER_LEG,Z,4,-3,2,
5,MotorAccel(MOTOR_RIGHT_ANKLE,9,1500,ACCEL_RIGHT_FOOT,Z,0,-8,15,ACCEL_RIGHT_UPPER_LEG,Z,2,-1,6,WAIT_STAGE|MAX_THRUST=9|MAX_REVERSE_THRUST=6|MAX_ACCEL=5|MIN_VELOCITY=30|MAX_VELOCITY=40|MAINTAIN_THRUST|ABORT_ANGLE1=30|ABORT_ON_TIMEOUT|FUTURE_LOG|REQUIRED)
#5,MotorAccel(MOTOR_RIGHT_ANKLE,7,1500,ACCEL_RIGHT_FOOT,Z,0,-8,8,ACCEL_RIGHT_UPPER_LEG,Z,2,-1,5,ACCEL_RIGHT_LOWER_LEG,Z,4,-3,10,WAIT_STAGE|MAX_THRUST=7|MAX_REVERSE_THRUST=3|MAX_ACCEL=10|MIN_VELOCITY=30|MAX_VELOCITY=40|NO_STOP_ON_TARGET|MAINTAIN_THRUST|NO_STOP_ON_TARGET|ABORT_ANGLE1=30|ABORT_ON_TIMEOUT|FUTURE_LOG)
#LAM is perhaps the most important motor at this stage, because there needs to be enough velocity to swing the Torso (and waist) to 3-5degrees
#5,MotorAccel(MOTOR_LEFT_ANKLE,7,1500,ACCEL_RIGHT_LOWER_LEG,Z,4,-3,5,ACCEL_RIGHT_UPPER_LEG,Z,3,-1,8,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=7|MAX_REVERSE_THRUST=5|MAX_ACCEL=5|MIN_VELOCITY=15|MAX_VELOCITY=70|MAINTAIN_THRUST|NO_STOP_ON_TARGET|FUTURE_LOG|ABORT_ANGLE0=30)
#LAM needs to give a strong thrust to lean the robot to the right
#ACCEL_TORSO,Z,7,-1,1,
#LAM needs to push RULZ to at least 1 degree, because RAM can reverse if RLLZ>6 but RULZ is still <-2
5,MotorAccel(MOTOR_LEFT_ANKLE,8,1500,ACCEL_LEFT_FOOT,Z,0,-10,5,ACCEL_RIGHT_UPPER_LEG,Z,3,-1,8,ACCEL_RIGHT_LOWER_LEG,Z,4,-3,8,WAIT_STAGE|MAX_THRUST=8|MAX_REVERSE_THRUST=5|MAX_ACCEL=5|MIN_VELOCITY=40|MAX_VELOCITY=50|MAINTAIN_THRUST|NO_STOP_ON_TARGET|FUTURE_LOG|ABORT_ANGLE1=30)
#sides
#stage 5 sometimes needs to gently push the TZ back to the left when the torso leans too far to the right to stop robot from falling right
5,MotorAccel(MOTOR_RIGHT_SIDE,3,1500,ACCEL_RIGHT_UPPER_LEG,Z,3,-2,2,ACCEL_TORSO,Z,4,-1,5,WAIT_STAGE|MIN_VELOCITY=20|MAX_VELOCITY=30|MAX_THRUST=3|MAX_REVERSE_THRUST=2|MAX_ACCEL=1)
#5,MotorAccel(MOTOR_RIGHT_SIDE,3,1500,ACCEL_TORSO,Z,4,-1,5,WAIT_STAGE|HOLD_ANGLE|MIN_VELOCITY=20|MAX_VELOCITY=30|MAX_THRUST=3|MAX_REVERSE_THRUST=2|MAX_ACCEL=1)
#ACCEL_RIGHT_UPPER_LEG,Z,3,-2,10, MIN_VELOCITY0=5|MAX_VELOCITY0=10|MAX_ACCEL0=1| 
#torso Z needs to go all the way to 7
#ankle needs to lean right before torso throws weight right
#Side motors need to copy stage 1 and shift weight over to the right side
#maxAccel=0.1 MinV=5 MaxV=7
#ULZ before LLZ because RLLZ tends to go very positive sooner 
##5,MotorAccel(MOTOR_LEFT_SIDE,5,1500,ACCEL_RIGHT_UPPER_LEG,Z,4.5,-1,5,ACCEL_RIGHT_LOWER_LEG,Z,4.5,-1,6,WAIT_STAGE|HOLD_ANGLE|MIN_VELOCITY=20|MAX_VELOCITY=60|MAX_THRUST=7|MAX_REVERSE_THRUST=2|MAX_ACCEL=5|ABORT_ON_TIMEOUT)
#RSM needs to be disabled in stage 5? so it can be ready for stage 6, otherwise it is going to be going in the wrong direction for stage 5 and need to suddenly reverse too quickly
##5,MotorAccel(MOTOR_RIGHT_SIDE,3,1500,ACCEL_RIGHT_UPPER_LEG,Z,4.5,-1,5,ACCEL_RIGHT_LOWER_LEG,Z,4.5,-1,2,WAIT_STAGE|HOLD_ANGLE|MIN_VELOCITY=20|MAX_VELOCITY=30|MAX_THRUST=7|MAX_REVERSE_THRUST=2|MAX_ACCEL=5)
#5,MotorAccel(MOTOR_RIGHT_SIDE,4,1500,ACCEL_TORSO,Z,7,-1,10,WAIT_STAGE|MAX_THRUST0=4|MAX_REVERSE_THRUST0=4|MAX_THRUST=8|MAX_REVERSE_THRUST=4|MIN_VELOCITY=20|MAX_VELOCITY=50|MAX_ACCEL1=5|FUTURE_LOG|NO_STOP_ON_TARGET|ABORT_ON_TIMEOUT)
#5,MotorAccel(MOTOR_LEFT_SIDE,4,1500,ACCEL_RIGHT_UPPER_LEG,Z,2,-1,5,ACCEL_TORSO,Z,5,-3,2,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=4|MAX_REVERSE_THRUST=4|MIN_VELOCITY=10|MAX_VELOCITY=30|MAX_ACCEL=3|NO_STOP_ON_TARGET)
#legs
5,MotorAccel(MOTOR_LEFT_LEG,3,1500,ACCEL_HIP,X,0,-1,1,WAIT_STAGE|TVR=0.5:1:3|MAX_THRUST=10|MAX_REVERSE_THRUST=3|MIN_VELOCITY=20|MAX_VELOCITY=40|MAX_ACCEL=1)
5,MotorAccel(MOTOR_RIGHT_LEG,3,1500,ACCEL_HIP,X,0,-1,1,WAIT_STAGE|TVR=0.5:1:3|MAX_THRUST=10|MAX_REVERSE_THRUST=3|MIN_VELOCITY=20|MAX_VELOCITY=40|MAX_ACCEL=1)
#note that the HIP Z is going to be leaning to the right around 5
#5,MotorAccel(MOTOR_RIGHT_KNEE,8,1500,ACCEL_HIP,Z,5,-3,3,ACCEL_HIP,X,0,-3,3,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST0=5|MAX_REVERSE_THRUST0=5|MAX_THRUST1=7|MAX_REVERSE_THRUST1=6|MAX_VELOCITY0=10|MAX_VELOCITY1=30|MAX_ACCEL=0.5|ABORT_ANGLE0=40|FARTHEST_FROM_TARGET|NO_MOTION_NO_THRUST)  
#5,MotorAccel(MOTOR_LEFT_KNEE,8,1500,ACCEL_HIP,Z,5,-3,3,ACCEL_HIP,X,0,-3,3,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST0=5|MAX_REVERSE_THRUST0=5|MAX_THRUST1=7|MAX_REVERSE_THRUST1=6|MAX_VELOCITY0=10|MAX_VELOCITY1=30|MAX_ACCEL=0.5|FARTHEST_FROM_TARGET|NO_MOTION_NO_THRUST|FUTURE_LOG)
#knees may need to help to turn hip (and torso) to 5 degrees - was just Hip X
#5,MotorAccel(MOTOR_RIGHT_KNEE,8,1500,ACCEL_HIP,X,0,-3,3,ACCEL_TORSO,Z,5,-2,2,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST0=7|MAX_REVERSE_THRUST0=6|MAX_VELOCITY0=30|MAX_ACCEL=0.5|ABORT_ANGLE0=40|MAX_THRUST1=7|MAX_REVERSE_THRUST1=6|MAX_VELOCITY1=30|MAX_ACCEL1=0.5|FARTHEST_FROM_TARGET|NO_MOTION_NO_THRUST)  
#5,MotorAccel(MOTOR_LEFT_KNEE,8,1500,ACCEL_HIP,X,0,-3,3,ACCEL_TORSO,Z,5,-2,2,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST0=7|MAX_REVERSE_THRUST0=6|MAX_VELOCITY0=30|MAX_ACCEL0=0.5|MAX_THRUST1=7|MAX_REVERSE_THRUST1=6|MAX_VELOCITY1=30|MAX_ACCEL1=0.5|FARTHEST_FROM_TARGET|NO_MOTION_NO_THRUST|FUTURE_LOG)
#knee needs to straighten here and fix ULXs
#FARTHEST_FROM_TARGET| - tends to bend the knee too farto fix the torso z
5,MotorAccel(MOTOR_RIGHT_KNEE,8,1500,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-1,1,ACCEL_TORSO,Z,5,-2,2,ACCEL_HIP,X,0,-3,3,WAIT_STAGE|TVR=2:0.5:5|MAX_THRUST0=14|MAX_REVERSE_THRUST0=6|MIN_VELOCITY0=20|MAX_VELOCITY0=30|MAX_ACCEL0=1|MAX_THRUST1=7|MAX_REVERSE_THRUST1=6|MAX_VELOCITY1=30|MAX_ACCEL1=0.5|ABORT_ANGLE1=40|MAX_THRUST2=7|MAX_REVERSE_THRUST2=6|MAX_VELOCITY2=30|MAX_ACCEL2=0.5|NO_MOTION_NO_THRUST)  
5,MotorAccel(MOTOR_LEFT_KNEE,10,1500,ACCEL_LEFT_UPPER_LEG,X,LULX0,-1,1,ACCEL_TORSO,Z,5,-2,2,ACCEL_HIP,X,0,-3,3,WAIT_STAGE|TVR=1:0.5:5|MAX_THRUST0=10|MAX_REVERSE_THRUST0=6|MIN_VELOCITY0=20|MAX_VELOCITY0=30|MAX_ACCEL0=1|MAX_THRUST1=10|MAX_REVERSE_THRUST1=6|MAX_VELOCITY1=30|MAX_ACCEL1=0.5|MAX_THRUST2=10|MAX_REVERSE_THRUST2=6|MAX_VELOCITY2=30|MAX_ACCEL2=0.5|FARTHEST_FROM_TARGET|NO_MOTION_NO_THRUST|FUTURE_LOG)
#NO_MOTION_NO_THRUST|
#feet
5,MotorAccel(MOTOR_LEFT_FOOT,10,1500,ACCEL_LEFT_FOOT,X,0,-5,5,ACCEL_LEFT_LOWER_LEG,X,LLLX0,-2,2,WAIT_STAGE|TVR=0.5:2:5|MAX_THRUST=12|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20|OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET)
5,MotorAccel(MOTOR_RIGHT_FOOT,10,1500,ACCEL_RIGHT_FOOT,X,0,-5,5,ACCEL_RIGHT_LOWER_LEG,X,RLLX0,-2,2,WAIT_STAGE|TVR=0.5:2:5|MAX_THRUST=12|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20|OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET)
#stage 5 torso gets thrown forward or backwards sometimes with a strong thrust
#0.2:0.15
#because knees straighten upper leg, and leg motors counter, torso needs to pull the torso forward here
#def thrown forwards
#note that MIN_VELOCITY (Min V towards target should be high enough to help the torso not fall back down)
#knee usually has to straighten leg and this throws torso forward
5,MotorAccel(MOTOR_TORSO,3,1500,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|TVR=0.4:1:3|MAX_THRUST=4|MAX_REVERSE_THRUST=2|MIN_VELOCITY=20|MAX_VELOCITY=40|MAX_ACCEL=2|NO_MOTION_NO_THRUST|OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET|FUTURE_LOG)
#possibly right leg motor needs to hold up leg or possibly falls down


#Stage 6: tilt torso to Z=+5 (similar to stage 2)
#no left side motor, because the motion can not be reversed fast enough in the next stage when left side motor may need to push the left leg o/ut
#use HIP Z? need large positive for LEG to make sure torso gets chosen for high velocity and thrust, and thrust direction is the same anyway (-)
#Torso Z was 5
#pulled out: ACCEL_RIGHT_UPPER_LEG,Z,3,-2,10,
#MAX_THRUST1=8|MAX_REVERSE_THRUST1=4|MIN_VELOCITY1=20|MAX_VELOCITY1=50|MAX_ACCEL1=5|
6,MotorAccel(MOTOR_RIGHT_SIDE,12,1000,ACCEL_TORSO,Z,3,-1,5,WAIT_STAGE|MAX_THRUST=12|MAX_REVERSE_THRUST=4|MIN_VELOCITY=30|MAX_VELOCITY=50|MAX_ACCEL=5|FUTURE_LOG|NO_STOP_ON_TARGET|ABORT_ON_TIMEOUT|REQUIRED)
#6,MotorAccel(MOTOR_RIGHT_SIDE,4,700,ACCEL_RIGHT_UPPER_LEG,Z,3,-2,10,ACCEL_TORSO,Z,3,-1,10,WAIT_STAGE|MAX_THRUST0=4|MAX_REVERSE_THRUST0=4|MIN_VELOCITY0=5|MAX_VELOCITY0=10|MAX_ACCEL0=1|MAX_THRUST1=8|MAX_REVERSE_THRUST1=4|MIN_VELOCITY1=20|MAX_VELOCITY1=50|MAX_ACCEL1=5|FUTURE_LOG|NO_STOP_ON_TARGET|ABORT_ON_TIMEOUT)
#6,MotorAccel(MOTOR_RIGHT_SIDE,4,700,ACCEL_TORSO,Z,5,-1,3,ACCEL_RIGHT_UPPER_LEG,Z,3,-1,6,WAIT_STAGE|MAX_THRUST1=4|MAX_REVERSE_THRUST1=4|MIN_VELOCITY1=30|MAX_VELOCITY1=100|MAX_THRUST0=13|MAX_REVERSE_THRUST0=4|MIN_VELOCITY0=30|MAX_VELOCITY0=50|MAX_ACCEL1=3|MAX_ACCEL0=10|FUTURE_LOG|NO_STOP_ON_TARGET|ABORT_ON_TIMEOUT)
#LEFT SIDE MOTOR CAN HELP TILT, but only to RULZ=0 and no Torso accel
#6,MotorAccel(MOTOR_LEFT_SIDE,4,700,ACCEL_RIGHT_UPPER_LEG,Z,0,-1,6,WAIT_STAGE|MAX_THRUST0=4|MAX_REVERSE_THRUST0=4|MIN_VELOCITY0=30|MAX_VELOCITY0=100|MAX_THRUST1=11|MAX_REVERSE_THRUST1=4|MAX_ACCEL0=3|NO_STOP_ON_TARGET)
#6,MotorAccel(MOTOR_LEFT_SIDE,4,700,ACCEL_TORSO,Z,5,-1,1,WAIT_STAGE|MIN_VELOCITY0=10|MAX_VELOCITY0=50|MAX_THRUST0=11|MAX_REVERSE_THRUST0=4|MAX_ACCEL0=3|NO_STOP_ON_TARGET)
#the legs need to push back the weight of the torso in the +X
#perhaps legs need to use Torso X but Hipx is less variable 
#stage 2 maxrevthrust=3 but sometimes need to break harder here
6,MotorAccel(MOTOR_LEFT_LEG,3,1000,ACCEL_HIP,X,0,-1,1,WAIT_STAGE|TVR=0.5:0.5:3|MAX_THRUST=6|MAX_REVERSE_THRUST=5|MAX_VELOCITY=40|MAX_ACCEL=1)
6,MotorAccel(MOTOR_RIGHT_LEG,3,1000,ACCEL_HIP,X,0,-1,1,WAIT_STAGE|TVR=0.5:0.5:3|MAX_THRUST=6|MAX_REVERSE_THRUST=5|MAX_VELOCITY=40|MAX_ACCEL=1)
#note that the HIP Z is going to be leaning to the right around 5
#6,MotorAccel(MOTOR_RIGHT_KNEE,8,700,ACCEL_HIP,Z,5,-3,3,ACCEL_HIP,X,0,-3,3,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST0=5|MAX_REVERSE_THRUST0=5|MAX_THRUST1=7|MAX_REVERSE_THRUST1=6|MAX_VELOCITY0=10|MAX_VELOCITY1=30|MAX_ACCEL=0.5|ABORT_ANGLE0=40|FARTHEST_FROM_TARGET|NO_MOTION_NO_THRUST)  
#6,MotorAccel(MOTOR_LEFT_KNEE,8,700,ACCEL_HIP,Z,5,-3,3,ACCEL_HIP,X,0,-3,3,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST0=5|MAX_REVERSE_THRUST0=5|MAX_THRUST1=7|MAX_REVERSE_THRUST1=6|MAX_VELOCITY0=10|MAX_VELOCITY1=30|MAX_ACCEL=0.5|FARTHEST_FROM_TARGET|NO_MOTION_NO_THRUST|FUTURE_LOG)
6,MotorAccel(MOTOR_RIGHT_KNEE,8,1000,ACCEL_HIP,X,0,-3,3,WAIT_STAGE|TVR=0.5:0.5:5|MAX_THRUST0=7|MAX_REVERSE_THRUST0=6|MAX_VELOCITY0=30|MAX_ACCEL=0.5|ABORT_ANGLE0=40|FARTHEST_FROM_TARGET|NO_MOTION_NO_THRUST)  
6,MotorAccel(MOTOR_LEFT_KNEE,8,1000,ACCEL_HIP,X,0,-3,3,WAIT_STAGE|TVR=0.5:0.5:5|MAX_THRUST0=7|MAX_REVERSE_THRUST0=6|MAX_VELOCITY0=30|MAX_ACCEL=0.5|FARTHEST_FROM_TARGET|NO_MOTION_NO_THRUST|FUTURE_LOG)
#NO_MOTION_NO_THRUST|
#ankles go to and hold lean right
#changed max thrust from 14 to 5 because too much can cause lower leg Z to go too far while upper body slowly catches up
#LAM needs to go back to flat after stage 5 pushing body to right
6,MotorAccel(MOTOR_LEFT_ANKLE,3,1000,ACCEL_LEFT_FOOT,Z,0,-5,5,ACCEL_RIGHT_LOWER_LEG,Z,7,-1,1,WAIT_STAGE|TVR=1:2:3|MAX_THRUST=7|MAX_REVERSE_THRUST=3|MAX_ACCEL=3|MIN_VELOCITY=30|MAX_VELOCITY=100|ABORT_ANGLE0=40)
#6,MotorAccel(MOTOR_RIGHT_ANKLE,3,700,ACCEL_RIGHT_FOOT,Z,0,-10,20,ACCEL_RIGHT_LOWER_LEG,Z,7,-1,1,WAIT_STAGE|HOLD_ANGLE=1:2|MAX_THRUST=7|MAX_REVERSE_THRUST=3|MAX_ACCEL=3|MIN_VELOCITY=30|MAX_VELOCITY=100|ABORT_ANGLE0=30|FUTURE_LOG)
#need to look at upper leg too
6,MotorAccel(MOTOR_RIGHT_ANKLE,3,1000,ACCEL_RIGHT_UPPER_LEG,Z,3,-2,2,ACCEL_RIGHT_LOWER_LEG,Z,7,-1,1,WAIT_STAGE|TVR=1:2:3|MAX_THRUST=7|MAX_REVERSE_THRUST=3|MAX_ACCEL=3|MIN_VELOCITY=30|MAX_VELOCITY=100|ABORT_ANGLE0=40|FUTURE_LOG)
#feet
6,MotorAccel(MOTOR_LEFT_FOOT,10,1000,ACCEL_LEFT_FOOT,X,0,-5,5,ACCEL_LEFT_LOWER_LEG,X,LLLX0,-2,2,WAIT_STAGE|TVR=0.5:2:5|MAX_THRUST=12|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20|NO_MOTION_NO_THRUST1|OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET)
6,MotorAccel(MOTOR_RIGHT_FOOT,10,1000,ACCEL_RIGHT_FOOT,X,0,-5,5,ACCEL_RIGHT_LOWER_LEG,X,RLLX0,-2,2,WAIT_STAGE|TVR=0.5:2:5|MAX_THRUST=12|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20|NO_MOTION_NO_THRUST1|OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET)
#stage 5 torso gets thrown forward with a strong thrust
6,MotorAccel(MOTOR_TORSO,3,1000,ACCEL_TORSO,X,TX0,-1,1,WAIT_STAGE|TVR=0.2:0.2:3|MAX_THRUST=6|MAX_REVERSE_THRUST=5|MIN_VELOCITY=10|MAX_VELOCITY=20|MAX_ACCEL=5|MAINTAIN_THRUST|NO_MOTION_NO_THRUST|OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET|FUTURE_LOG)
#possibly right leg motor needs to hold up leg or possibly falls down


#Stage 7: Hold lean right, take left step up - similar to stage 3
#angle was 18 (stage 3 has 15 (-1,5))
#6->15, is now 12->21
7,MotorAccel(MOTOR_LEFT_LEG,14,400,ACCEL_LEFT_UPPER_LEG,X,21,-1,5,WAIT_STAGE|CONSTANT_THRUST|REQUIRED)
7,MotorAccel(MOTOR_RIGHT_LEG,14,400,ACCEL_LEFT_UPPER_LEG,X,21,-1,5,WAIT_STAGE|CONSTANT_THRUST)
7,MotorAccel(MOTOR_LEFT_KNEE,20,400,ACCEL_LEFT_LOWER_LEG,X,-30,-1,1,WAIT_STAGE|CONSTANT_THRUST)
7,MotorAccel(MOTOR_RIGHT_KNEE,3,400,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-2,2,WAIT_STAGE|TVR=0.5:0.5:3|MAX_THRUST=9|MAX_REVERSE_THRUST=6|MAINTAIN_THRUST|ABORT_ANGLE0=25)
#ACCEL_RIGHT_LOWER_LEG,Z,7,-1,1,
7,MotorAccel(MOTOR_RIGHT_ANKLE,19,400,ACCEL_RIGHT_FOOT,Z,0,-10,8,ACCEL_RIGHT_UPPER_LEG,Z,4,-1,1,WAIT_STAGE|TVR=2.0:20.0:8|MAX_THRUST=19|MAX_REVERSE_THRUST=2|MIN_VELOCITY=5|MAX_VELOCITY=20|MAX_ACCEL=0.5|ABORT_ANGLE0=30|OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET|FUTURE_LOG)
#7,MotorAccel(MOTOR_RIGHT_ANKLE,20,400,ACCEL_RIGHT_FOOT,Z,0,-10,8,ACCEL_RIGHT_LOWER_LEG,Z,7,-1,1,ACCEL_RIGHT_UPPER_LEG,Z,4,-1,1,WAIT_STAGE|HOLD_ANGLE=2.0:20.0|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MIN_VELOCITY=5|MAX_VELOCITY=20|MAX_ACCEL=0.5|ABORT_ANGLE0=30|OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET|FUTURE_LOG)
#7,MotorAccel(MOTOR_RIGHT_ANKLE,20,400,ACCEL_RIGHT_FOOT,Z,0,-15,10,ACCEL_RIGHT_UPPER_LEG,Z,3,-1,1,ACCEL_RIGHT_LOWER_LEG,Z,4,-3,2,WAIT_STAGE|HOLD_ANGLE=1.0:10.0|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MIN_VELOCITY=5|MAX_VELOCITY=20|MAX_ACCEL=0.5|MAINTAIN_THRUST|ABORT_ANGLE0=30|OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET|FUTURE_LOG)
7,MotorAccel(MOTOR_LEFT_ANKLE,3,400,ACCEL_LEFT_FOOT,Z,0,-5,10,ACCEL_RIGHT_LOWER_LEG,Z,7,-1,1,WAIT_STAGE|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MAX_VELOCITY=5|MAX_ACCEL=0.5)
#use HIP Z?
#important to right side to swing the torso right or left if necessary 9 is too far
#ACCEL_RIGHT_LOWER_LEG,Z,7,-7,2, MIN_VELOCITY0=5|MAX_VELOCITY0=10|MAX_ACCEL0=1|
7,MotorAccel(MOTOR_RIGHT_SIDE,19,400,ACCEL_TORSO,Z,7,-1,1,WAIT_STAGE|TVR=0:1:8|MAX_THRUST=19|MAX_REVERSE_THRUST=2|MIN_VELOCITY=20|MAX_VELOCITY=50|MAX_ACCEL=5.0|MAINTAIN_THRUST|NO_STOP_ON_TARGET|FUTURE_LOG)
#LSM needs to monitor leg separation in addition to help to bring LULX up
7,MotorAccel(MOTOR_LEFT_SIDE,19,400,ACCEL_LEFT_UPPER_LEG,Z,7,-1,1,ACCEL_RIGHT_UPPER_LEG,Z,0,0,0,ACCEL_LEFT_UPPER_LEG,X,21,-1,5,WAIT_STAGE|TVR=0:20:8|MAX_THRUST=19|MAX_REVERSE_THRUST=2|MIN_VELOCITY=80|MAX_VELOCITY=100|MAX_ACCEL=10.0|NO_STOP_ON_TARGET|RELATIVE_ANGLE0|MAINTAIN_THRUST)
#7,MotorAccel(MOTOR_LEFT_SIDE,13,400,ACCEL_LEFT_UPPER_LEG,Z,7,-1,1,ACCEL_RIGHT_UPPER_LEG,Z,0,0,0,WAIT_STAGE|HOLD_ANGLE=0:20|MAX_THRUST=10|MAX_REVERSE_THRUST=2|MIN_VELOCITY=5|MAX_VELOCITY=40|MAX_ACCEL=3.0|NO_STOP_ON_TARGET|RELATIVE_ANGLE0|MAINTAIN_THRUST)
7,MotorAccel(MOTOR_RIGHT_FOOT,5,400,ACCEL_RIGHT_FOOT,X,0,-10,10,ACCEL_RIGHT_LOWER_LEG,X,RLLX0,-1,1,WAIT_STAGE|TVR=0.5:2:5|ABORT_ANGLE1=-30|MAX_THRUST=16|MAX_REVERSE_THRUST=3|MAX_ACCEL=1|MAX_VELOCITY=20)
7,MotorAccel(MOTOR_LEFT_FOOT,10,400,ACCEL_LEFT_FOOT,X,0,-5,5,WAIT_STAGE|MAX_THRUST=12|MAX_REVERSE_THRUST=3|MAX_VELOCITY=5)
#torso needs to push back
7,MotorAccel(MOTOR_TORSO,3,400,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|TVR=0.2:0.4:3|MAX_THRUST=9|MAX_REVERSE_THRUST=2|MIN_VELOCITY=5|MAX_VELOCITY=10|MAX_ACCEL=1|NO_MOTION_NO_THRUST|OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET|MAINTAIN_THRUST)


#Stage 8: Left step down, similar to stage 4, right leans back to the left
#end stage if Right foot front or back touch sensors > 3
#required to end stage 4: RULX reaches RULX0 +-1 or Right foot front or back Touch Sensor > 3
8,TouchSensor(TOUCH_LEFT_FOOT_FRONT,GT,50.0,250,800)
8,TouchSensor(TOUCH_LEFT_FOOT_BACK,GT,50.0,250,800)
#stage 4 should not end until the HIP Z is 2-4, and the LUL Z is around 3. MIN_VELOCITY is important- controls speed in fall to right
8,MotorAccel(MOTOR_RIGHT_ANKLE,3,800,ACCEL_RIGHT_FOOT,Z,0,-15,10,ACCEL_RIGHT_UPPER_LEG,Z,RULZ0,-2,2,ACCEL_RIGHT_LOWER_LEG,Z,RLLZ0,-2,2,ACCEL_HIP,Z,0,-2,2,WAIT_STAGE|MAX_THRUST=10|MAX_REVERSE_THRUST=2|MIN_VELOCITY=30|MAX_VELOCITY=50|MAX_ACCEL=5|FUTURE_LOG|ABORT_ANGLE2=30|MAINTAIN_THRUST|REQUIRED)
8,MotorAccel(MOTOR_LEFT_LEG,12,800,ACCEL_LEFT_UPPER_LEG,X,LULX0,-1,1,WAIT_STAGE|CONSTANT_THRUST)
8,MotorAccel(MOTOR_RIGHT_LEG,12,800,ACCEL_LEFT_UPPER_LEG,X,LULX0,-1,1,WAIT_STAGE|MAX_THRUST=18|MAX_REVERSE_THRUST=3|CONSTANT_THRUST)
8,MotorAccel(MOTOR_LEFT_KNEE,20,800,ACCEL_LEFT_LOWER_LEG,X,-24,-1,1,ACCEL_LEFT_UPPER_LEG,X,0,0,0,WAIT_STAGE|RELATIVE_ANGLE0|ABORT_ANGLE0=-40|MAX_ACCEL=10|MIN_VELOCITY=30|MAX_VELOCITY=50|MAX_THRUST=20|CONSTANT_THRUST20:4)
##8,MotorAccel(MOTOR_RIGHT_ANKLE,3,800,ACCEL_RIGHT_FOOT,Z,0,-10,10,ACCEL_RIGHT_LOWER_LEG,Z,RLLZ0,-1,1,WAIT_STAGE|HOLD_ANGLE=2:10|MAX_THRUST=20|MAX_REVERSE_THRUST=2|ABORT_ANGLE0=40|MAINTAIN_THRUST)
#z leg spacing is most important in stage 4 (but trying to keep torso Z to the left helps stage 5 to lean right, and serves as a counter for the RSM thrust in trying to keep legs apart anyway)
8,MotorAccel(MOTOR_RIGHT_SIDE,6,800,ACCEL_TORSO,Z,0,-1,1,ACCEL_RIGHT_UPPER_LEG,Z,7,-1,1,ACCEL_LEFT_UPPER_LEG,Z,0,0,0,WAIT_STAGE|TVR=1:1:6|MAX_THRUST=7|MAX_REVERSE_THRUST=2|MAX_ACCEL=5|MIN_VELOCITY=10|MAX_VELOCITY=20|RELATIVE_ANGLE1)
#8,MotorAccel(MOTOR_RIGHT_SIDE,6,800,ACCEL_TORSO,Z,0,-2,2,ACCEL_RIGHT_UPPER_LEG,Z,-8,-1,1,ACCEL_LEFT_UPPER_LEG,Z,0,0,0,WAIT_STAGE|HOLD_ANGLE=0:10|MAX_THRUST=10|MIN_VELOCITY=20|MAX_VELOCITY=40|MAX_ACCEL=5|MAX_REVERSE_THRUST=2|MAINTAIN_THRUST|RELATIVE_ANGLE1|FUTURE_LOG)
#note that second relative angle helps to keep LSM thrusting when LLLZ trails behind LULZ
#LSM needs to monitor leg separation in addition to help to bring LULX down
8,MotorAccel(MOTOR_LEFT_SIDE,8,800,ACCEL_LEFT_UPPER_LEG,Z,7,-1,1,ACCEL_RIGHT_UPPER_LEG,Z,0,0,0,ACCEL_LEFT_UPPER_LEG,Z,LULX0,-2,2,WAIT_STAGE|TVR=1:10:6|MAX_THRUST=8|MAX_REVERSE_THRUST=2|MIN_VELOCITY=10|MAX_VELOCITY=30|MAX_ACCEL=3|RELATIVE_ANGLE0)
#8,MotorAccel(MOTOR_LEFT_SIDE,8,800,ACCEL_LEFT_UPPER_LEG,Z,7,-1,1,ACCEL_RIGHT_UPPER_LEG,Z,0,0,0,ACCEL_LEFT_LOWER_LEG,Z,0,-1,1,ACCEL_LEFT_UPPER_LEG,Z,0,0,0,WAIT_STAGE|HOLD_ANGLE=1:10|MAX_THRUST=8|MAX_REVERSE_THRUST=2|MIN_VELOCITY=10|MAX_VELOCITY=30|MAX_ACCEL=3|RELATIVE_ANGLE0|RELATIVE_ANGLE2)
#8,MotorAccel(MOTOR_LEFT_SIDE,14,800,ACCEL_LEFT_UPPER_LEG,Z,7,-1,1,ACCEL_RIGHT_UPPER_LEG,Z,0,0,0,WAIT_STAGE|HOLD_ANGLE=0:10|MAX_THRUST=14|MAX_REVERSE_THRUST=2|MAX_ACCEL=1|RELATIVE_ANGLE0)
8,MotorAccel(MOTOR_RIGHT_FOOT,10,800,ACCEL_RIGHT_FOOT,X,0,-10,10,ACCEL_RIGHT_LOWER_LEG,X,RLLX0,-1,1,WAIT_STAGE|TVR=2:2:5|MAX_THRUST=16|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20)
8,MotorAccel(MOTOR_RIGHT_KNEE,10,800,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-2,2,ACCEL_HIP,Z,0,-2,2,WAIT_STAGE|TVR=0.5:0.5:5|MAX_THRUST=12|MAX_REVERSE_THRUST=6|MAINTAIN_THRUST|ABORT_ANGLE0=30)
8,MotorAccel(MOTOR_LEFT_ANKLE,10,800,ACCEL_LEFT_FOOT,Z,0,-10,10,WAIT_STAGE|TVR=1:1:5|MAX_THRUST=18|MAX_REVERSE_THRUST=6)
8,MotorAccel(MOTOR_LEFT_FOOT,10,800,ACCEL_LEFT_FOOT,X,0,-2,2,WAIT_STAGE|TVR=1:1:5|MAX_THRUST=12|MAX_REVERSE_THRUST=6)
8,MotorAccel(MOTOR_TORSO,3,800,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|TVR=0.2:0.2:3|MAX_THRUST=4|MAX_REVERSE_THRUST=2|MIN_VELOCITY=3|MAX_VELOCITY=5|MAX_ACCEL=1|NO_MOTION_NO_THRUST|OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET|MAINTAIN_THRUST)


#Stage 9: BALANCE stand for 5 seconds
#(Basically try to lean back to the center and hold those angles)
9,MotorAccel(MOTOR_LEFT_SIDE,3,5000,ACCEL_LEFT_UPPER_LEG,Z,LULZ0,-3,3,ACCEL_HIP,Z,0,-2,2,WAIT_STAGE|TVR=0.5:0.5:3|MAX_THRUST=7|MAX_REVERSE_THRUST=3|MIN_VELOCITY=10|MAX_VELOCITY=15|MAX_ACCEL=1|FARTHEST_FROM_TARGET|NO_MOTION_NO_THRUST)
9,MotorAccel(MOTOR_RIGHT_SIDE,3,5000,ACCEL_RIGHT_UPPER_LEG,Z,RULZ0,-3,3,ACCEL_HIP,Z,0,-2,2,WAIT_STAGE|TVR=0.5:0.5:3|MAX_THRUST=7|MAX_REVERSE_THRUST=3|MIN_VELOCITY=10|MAX_VELOCITY=15|MAX_ACCEL=1|FARTHEST_FROM_TARGET|NO_MOTION_NO_THRUST)
#the legs need to push back the weight of the torso in the +X
#perhaps legs need to use Torso X but Hipx is less variable 
9,MotorAccel(MOTOR_LEFT_LEG,5,5000,ACCEL_HIP,X,0,-1,1,WAIT_STAGE|TVR=0.5:0.5:4|MAX_THRUST=7|MAX_REVERSE_THRUST=3|MAX_VELOCITY=40|MAX_ACCEL=1)
9,MotorAccel(MOTOR_RIGHT_LEG,5,5000,ACCEL_HIP,X,0,-1,1,WAIT_STAGE|TVR=0.5:0.5:4|MAX_THRUST=7|MAX_REVERSE_THRUST=3|MAX_VELOCITY=40|MAX_ACCEL=1)
9,MotorAccel(MOTOR_RIGHT_KNEE,9,5000,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-2,2,ACCEL_HIP,Z,0,-3,3,ACCEL_HIP,X,0,-3,3,WAIT_STAGE|TVR=0.5:0.5:5|MAX_THRUST0=9|MAX_REVERSE_THRUST0=6|MAX_THRUST1=5|MAX_REVERSE_THRUST1=5|MAX_THRUST0=9|MAX_REVERSE_THRUST0=6|MAX_THRUST2=7|MAX_REVERSE_THRUST2=6|MAX_VELOCITY=10|MAX_ACCEL=0.5|ABORT_ANGLE1=40|FARTHEST_FROM_TARGET|NO_MOTION_NO_THRUST)  
9,MotorAccel(MOTOR_LEFT_KNEE,9,5000,ACCEL_LEFT_UPPER_LEG,X,LULX0,-2,2,ACCEL_HIP,Z,0,-3,3,ACCEL_HIP,X,0,-3,3,WAIT_STAGE|TVR=0.5:0.5:5|MAX_THRUST0=9|MAX_REVERSE_THRUST0=6|MAX_THRUST1=5|MAX_REVERSE_THRUST1=5|MAX_THRUST0=9|MAX_REVERSE_THRUST0=6|MAX_THRUST2=7|MAX_REVERSE_THRUST2=6|MAX_VELOCITY=10|MAX_ACCEL=0.5|FARTHEST_FROM_TARGET|NO_MOTION_NO_THRUST|FUTURE_LOG)
#9,MotorAccel(MOTOR_RIGHT_KNEE,7,5000,ACCEL_HIP,Z,0,-3,3,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-2,2,ACCEL_HIP,X,0,-3,3,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST0=5|MAX_REVERSE_THRUST0=5|MAX_THRUST1=7|MAX_REVERSE_THRUST1=6|MAX_THRUST2=7|MAX_REVERSE_THRUST2=6|MAX_VELOCITY0=10|MAX_VELOCITY=10|MAX_ACCEL=0.5|ABORT_ANGLE0=40|FARTHEST_FROM_TARGET|NO_MOTION_NO_THRUST)  
#9,MotorAccel(MOTOR_LEFT_KNEE,7,5000,ACCEL_HIP,Z,0,-3,3,ACCEL_LEFT_UPPER_LEG,X,LULX0,-2,2,ACCEL_HIP,X,0,-3,3,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST0=5|MAX_REVERSE_THRUST0=5|MAX_THRUST1=7|MAX_REVERSE_THRUST1=6|MAX_THRUST2=7|MAX_REVERSE_THRUST2=6|MAX_VELOCITY0=10|MAX_VELOCITY1=30|MAX_ACCEL=0.5|FARTHEST_FROM_TARGET|NO_MOTION_NO_THRUST|FUTURE_LOG)
#NO_MOTION_NO_THRUST|
9,MotorAccel(MOTOR_LEFT_ANKLE,3,5000,ACCEL_LEFT_FOOT,Z,0,-10,10,ACCEL_LEFT_LOWER_LEG,Z,LLLZ0,-3,3,WAIT_STAGE|TVR=1:2:3|MAX_THRUST=14|MAX_REVERSE_THRUST=3|MAX_ACCEL=1|MIN_VELOCITY=20|MAX_VELOCITY=30|NO_MOTION_NO_THRUST1|ABORT_ANGLE0=30)
#RAM needs to push right side back to left RAM=- RULZ0=-3
#right foot Z can become >10 so set to 20 just to make sure right foot will be countering landing (which requires RAM-, RAM+ corrects RFZ>10) - need a high MinV and accel
9,MotorAccel(MOTOR_RIGHT_ANKLE,3,5000,ACCEL_RIGHT_FOOT,Z,0,-10,20,ACCEL_RIGHT_LOWER_LEG,Z,RLLZ0,-3,3,WAIT_STAGE|TVR=1:2:3|MAX_THRUST=14|MAX_REVERSE_THRUST=3|MAX_ACCEL=1|MIN_VELOCITY=20|MAX_VELOCITY=30|NO_MOTION_NO_THRUST1|ABORT_ANGLE0=30|FUTURE_LOG)
#RELATIVE_ANGLE0 could be used for Foot X with LLX
#keep feet Z close to ground
9,MotorAccel(MOTOR_LEFT_FOOT,10,5000,ACCEL_LEFT_FOOT,X,0,-7,7,ACCEL_LEFT_LOWER_LEG,X,LLLX0,-2,2,WAIT_STAGE|TVR=0.25:1:5|MAX_THRUST=11|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20|NO_MOTION_NO_THRUST1|OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET)
9,MotorAccel(MOTOR_RIGHT_FOOT,10,5000,ACCEL_RIGHT_FOOT,X,0,-7,7,ACCEL_RIGHT_LOWER_LEG,X,RLLX0,-2,2,WAIT_STAGE|TVR=0.25:1:5|MAX_THRUST=10|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20|NO_MOTION_NO_THRUST1|OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET)
9,MotorAccel(MOTOR_TORSO,3,5000,ACCEL_TORSO,X,TX0,-1,1,WAIT_STAGE|TVR=0.2:0.4:3|MAX_THRUST=7|MAX_REVERSE_THRUST=2|MIN_VELOCITY=3|MAX_VELOCITY=10|MAX_ACCEL=5|NO_MOTION_NO_THRUST|OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET|FUTURE_LOG)
#possibly right leg motor needs to hold up leg or possibly falls down

12000,StopLogging

