#Right2StepUpDown01.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
#for a long time had MAX_ACCEL=0.1 for both side and anklemotors - moves very controlably, mixV=5 maxV=7
#do not MAINTAIN_THRUST| because left side needs to thrust strongly from the start
1,MotorAccel(MOTOR_LEFT_SIDE,3,5000,ACCEL_LEFT_LOWER_LEG,Z,-4,-6,1,ACCEL_LEFT_UPPER_LEG,Z,-4,-5,1,WAIT_STAGE|MIN_VELOCITY=8|MAX_VELOCITY=10|MAX_THRUST=3|MAX_REVERSE_THRUST=5|MAX_ACCEL=0.5|NO_STOP_ON_TARGET|ABORT_ON_TIMEOUT|FUTURE_LOG)
1,MotorAccel(MOTOR_RIGHT_SIDE,3,5000,ACCEL_LEFT_LOWER_LEG,Z,-4,-2,1,ACCEL_LEFT_UPPER_LEG,Z,-4,-5,1,WAIT_STAGE|HOLD_ANGLE|MIN_VELOCITY=8|MAX_VELOCITY=10|MAX_THRUST=3|MAX_REVERSE_THRUST=5|MAX_ACCEL=0.5)
#|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,-2,1,ACCEL_LEFT_UPPER_LEG,Z,-3,-2,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=5|MAX_REVERSE_THRUST=5|MIN_VELOCITY=5|MAX_VELOCITY=8|MAX_ACCEL=0.3|FUTURE_LOG|MAINTAIN_THRUST)
1,MotorAccel(MOTOR_RIGHT_ANKLE,3,5000,ACCEL_LEFT_LOWER_LEG,Z,-4,-2,1,ACCEL_LEFT_UPPER_LEG,Z,-3,-2,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=5|MAX_REVERSE_THRUST=5|MIN_VELOCITY=5|MAX_VELOCITY=8|MAX_ACCEL=0.3|MAINTAIN_THRUST)
1,MotorAccel(MOTOR_TORSO,1,5000,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.15:1|MAX_THRUST=4|MAX_REVERSE_THRUST=2|MAINTAIN_THRUST|MAX_VELOCITY=10|MIN_ACCEL=1|NO_MOTION_NO_THRUST|OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET|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
#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 help but only to LULZ=0
#MinV=5 MaxV=8 MaxA=0.1 MinV0=5 MaxV0=8 MaxA0=0.5
2,MotorAccel(MOTOR_LEFT_SIDE,7,4000,ACCEL_LEFT_UPPER_LEG,Z,-3,-6,1,ACCEL_TORSO,Z,-5,-1,1,WAIT_STAGE|MAX_THRUST0=4|MAX_REVERSE_THRUST0=4|MIN_VELOCITY0=8|MAX_VELOCITY0=10|MAX_THRUST1=11|MAX_REVERSE_THRUST1=4|MIN_VELOCITY1=30|MAX_VELOCITY1=50|MAX_ACCEL0=1|MAX_ACCEL1=10|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), 
#MaxA=0.1 MinV=5 MaxV=8 MaxT=6 MaxRT=6
#ALLL -5 -2 1  ALUL -4 -3 1
#LAM really needs to hold up left leg
#LLLZ of -8 is a little too far to lean
2,MotorAccel(MOTOR_LEFT_ANKLE,4,4000,ACCEL_LEFT_FOOT,Z,0,-10,10,ACCEL_LEFT_LOWER_LEG,Z,-4,-3,1,ACCEL_LEFT_UPPER_LEG,Z,-4,-4,1,WAIT_STAGE|MAX_THRUST=10|MAX_REVERSE_THRUST=7|MIN_VELOCITY=5|MAX_VELOCITY=8|ABORT_ANGLE0=30|HOLD_ANGLE=20:0.5|MAX_ACCEL=0.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,-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,4000,ACCEL_LEFT_LOWER_LEG,Z,-5,-2,1,ACCEL_LEFT_UPPER_LEG,Z,-4,-3,1,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)
#|NO_STOP_ON_TARGET
#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=0.5:1.5|MAX_THRUST=4|MAX_REVERSE_THRUST=2|MAINTAIN_THRUST|MAX_VELOCITY=10|MIN_ACCEL=1|NO_MOTION_NO_THRUST|OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET|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.
3,MotorAccel(MOTOR_RIGHT_LEG,12,400,ACCEL_RIGHT_UPPER_LEG,X,15,-1,5,WAIT_STAGE|CONSTANT_THRUST)
3,MotorAccel(MOTOR_LEFT_LEG,12,400,ACCEL_RIGHT_UPPER_LEG,X,15,-1,5,WAIT_STAGE|CONSTANT_THRUST|HOLD_ANGLE)
#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)
3,MotorAccel(MOTOR_LEFT_KNEE,3,400,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)
#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
3,MotorAccel(MOTOR_LEFT_ANKLE,12,400,ACCEL_LEFT_FOOT,Z,0,-10,15,ACCEL_LEFT_LOWER_LEG,Z,-6,-2,1,WAIT_STAGE|HOLD_ANGLE=10.0:2.0|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MAX_VELOCITY=5|MAX_ACCEL=0.5|MAINTAIN_THRUST|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,-6,-2,1,WAIT_STAGE|HOLD_ANGLE|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
#LSM is pretty critical for when the torso Z will fall right- not until stage 4
#ALLLZ -7,-2,7
3,MotorAccel(MOTOR_LEFT_SIDE,12,400,ACCEL_LEFT_LOWER_LEG,Z,-7,-2,7,ACCEL_TORSO,Z,-5,-1,1,WAIT_STAGE|HOLD_ANGLE=40:0|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MIN_VELOCITY=20|MAX_VELOCITY=50|MAX_ACCEL=5.0|MAINTAIN_THRUST|NO_STOP_ON_TARGET|FUTURE_LOG)
#3,MotorAccel(MOTOR_LEFT_SIDE,12,400,ACCEL_LEFT_LOWER_LEG,Z,7,-1,1,ACCEL_RIGHT_UPPER_LEG,Z,0,0,0,ACCEL_TORSO,Z,-6,-1,1,WAIT_STAGE|HOLD_ANGLE=40:0|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MIN_VELOCITY=20|MAX_VELOCITY=50|MAX_ACCEL=5.0|MAINTAIN_THRUST|NO_STOP_ON_TARGET|RELATIVE_ANGLE0|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
3,MotorAccel(MOTOR_RIGHT_SIDE,10,400,ACCEL_RIGHT_UPPER_LEG,Z,-6,-1,1,ACCEL_LEFT_UPPER_LEG,Z,0,0,0,WAIT_STAGE|HOLD_ANGLE=20:0|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)
#send LLL forward slightly to stop fall backwards -12 to -14
3,MotorAccel(MOTOR_LEFT_FOOT,5,400,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)
#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,0,-5,5,WAIT_STAGE|HOLD_ANGLE|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
3,MotorAccel(MOTOR_TORSO,3,400,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.2:0.4|MAX_THRUST=6|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 4: LOWER RIGHT LEG SWINGS BACK, UPPER LEG COMES BACK DOWN, left side holds right leg off the ground 
#***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,3.0,250,400)
##4,TouchSensor(TOUCH_RIGHT_FOOT_BACK,GT,3.0,250,400)
#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
4,MotorAccel(MOTOR_LEFT_ANKLE,3,400,ACCEL_LEFT_FOOT,Z,0,-10,10,ACCEL_LEFT_UPPER_LEG,Z,3,-1,10,ACCEL_HIP,Z,3,-1,5,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)
#
4,MotorAccel(MOTOR_RIGHT_LEG,12,400,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-2,2,WAIT_STAGE|HOLD_ANGLE|CONSTANT_THRUST)
4,MotorAccel(MOTOR_LEFT_LEG,12,400,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-2,2,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
#target angle was RLLX0 (-12), but needs to be relative angle (RULX-18  so when RULX=21 LULX target 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,20,400,ACCEL_RIGHT_LOWER_LEG,X,-18,-1,1,ACCEL_RIGHT_UPPER_LEG,X,0,0,0,WAIT_STAGE|HOLD_ANGLE|RELATIVE_ANGLE0|ABORT_ANGLE0=-40|MAX_ACCEL=10|MIN_VELOCITY=30|MAX_VELOCITY=50|MAX_THRUST=20|CONSTANT_THRUST20:4)
#4,MotorAccel(MOTOR_LEFT_ANKLE,3,400,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,400,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,400,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,400,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,400,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
#4,MotorAccel(MOTOR_LEFT_SIDE,6,400,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
4,MotorAccel(MOTOR_LEFT_SIDE,6,400,ACCEL_LEFT_UPPER_LEG,Z,7,-2,3,ACCEL_RIGHT_UPPER_LEG,Z,0,0,0,ACCEL_TORSO,Z,5,-1,1,WAIT_STAGE|HOLD_ANGLE|RELATIVE_ANGLE0|MAX_THRUST=5|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,400,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
4,MotorAccel(MOTOR_RIGHT_SIDE,12,400,ACCEL_RIGHT_UPPER_LEG,Z,-7,-1,1,ACCEL_LEFT_UPPER_LEG,Z,0,0,0,WAIT_STAGE|HOLD_ANGLE=10:2|MAX_THRUST=12|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,400,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)
#4,MotorAccel(MOTOR_LEFT_KNEE,10,400,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,400,ACCEL_HIP,X,0,-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)
#right ankle and right foot motors should straighten right foot - add a little (-2) so right ankle does not stop at 0
4,MotorAccel(MOTOR_RIGHT_ANKLE,10,400,ACCEL_RIGHT_FOOT,Z,-4,-2,2,WAIT_STAGE|HOLD_ANGLE=1:1|MAX_THRUST=18|MAX_REVERSE_THRUST=6)
4,MotorAccel(MOTOR_RIGHT_FOOT,10,400,ACCEL_RIGHT_FOOT,X,0,-2,2,WAIT_STAGE|HOLD_ANGLE=1:1|MAX_THRUST=12|MAX_REVERSE_THRUST=6)
#still a need to hold torso forward against thrust making torso fall backward
#=1:1
#cannot have flag: OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET| because will thrust when moving against gravity
4,MotorAccel(MOTOR_TORSO,3,400,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.4:0.2|MAX_THRUST=5|MAX_REVERSE_THRUST=2|MIN_VELOCITY=3|MAX_VELOCITY=5|MAX_ACCEL=5|NO_MOTION_NO_THRUST|OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET|MAINTAIN_THRUST)


#Stage 5: Robot falls right, (similar to stages 1)
#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
5,MotorAccel(MOTOR_RIGHT_ANKLE,7,2000,ACCEL_RIGHT_LOWER_LEG,Z,4,-3,4,ACCEL_RIGHT_UPPER_LEG,Z,2,-1,7,WAIT_STAGE|MAX_THRUST=7|MAX_REVERSE_THRUST=5|MAX_ACCEL=5|MIN_VELOCITY=15|MAX_VELOCITY=70|NO_STOP_ON_TARGET|MAINTAIN_THRUST|NO_STOP_ON_TARGET|ABORT_ANGLE0=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,2000,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
5,MotorAccel(MOTOR_LEFT_ANKLE,7,2000,ACCEL_TORSO,Z,5,-1,1,ACCEL_RIGHT_LOWER_LEG,Z,4,-3,5,ACCEL_RIGHT_UPPER_LEG,Z,3,-1,8,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=10|MAX_REVERSE_THRUST=5|MAX_ACCEL=5|MIN_VELOCITY=15|MAX_VELOCITY=70|MAINTAIN_THRUST|NO_STOP_ON_TARGET|FUTURE_LOG|ABORT_ANGLE1=30)
#sides
#5,MotorAccel(MOTOR_RIGHT_SIDE,4,2000,ACCEL_RIGHT_LOWER_LEG,Z,5,-1,5,ACCEL_RIGHT_UPPER_LEG,Z,3,-1,3,WAIT_STAGE|HOLD_ANGLE}MAX_THRUST=4|MAX_REVERSE_THRUST=4|MIN_VELOCITY=10|MAX_VELOCITY=30|MAX_ACCEL0=3|NO_STOP_ON_TARGET|FUTURE_LOG)
#SIDE MOTORS are important in stage 5- I think the stage 1 approach of reversing the motors cannot really work for this stage- the ankles need to throw the upper body weight to the right, then the right side motor throws the torso weight over the right leg in stage 6 
#5,MotorAccel(MOTOR_RIGHT_SIDE,4,2000,ACCEL_TORSO,Z,5,-1,2,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=7|MAX_REVERSE_THRUST=4|MIN_VELOCITY=20|MAX_VELOCITY=50|MAX_ACCEL0=5|NO_STOP_ON_TARGET|FUTURE_LOG)
#5,MotorAccel(MOTOR_RIGHT_SIDE,4,2000,ACCEL_RIGHT_LOWER_LEG,Z,2,-1,5,ACCEL_RIGHT_UPPER_LEG,Z,2,-1,3,ACCEL_TORSO,Z,5,-1,2,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=7|MAX_REVERSE_THRUST=4|MIN_VELOCITY=20|MAX_VELOCITY=50|MAX_ACCEL2=5|NO_STOP_ON_TARGET|FUTURE_LOG)
#5,MotorAccel(MOTOR_RIGHT_SIDE,4,2000,ACCEL_RIGHT_UPPER_LEG,Z,3,-2,10,ACCEL_TORSO,Z,5,-1,3,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=7|MAX_REVERSE_THRUST=4|MIN_VELOCITY=20|MAX_VELOCITY=50|MAX_ACCEL0=5|NO_STOP_ON_TARGET|FUTURE_LOG)
#LSM helps to make sure TZ does not fall too negative (not past -5 range:-5<=>6)- 
#5,MotorAccel(MOTOR_LEFT_SIDE,4,2000,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,2000,ACCEL_HIP,X,0,-1,1,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=6|MAX_REVERSE_THRUST=3|MAX_VELOCITY=40|MAX_ACCEL=1)
5,MotorAccel(MOTOR_RIGHT_LEG,3,2000,ACCEL_HIP,X,0,-1,1,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=6|MAX_REVERSE_THRUST=3|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,2000,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,2000,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,2000,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,2000,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)
5,MotorAccel(MOTOR_RIGHT_KNEE,8,2000,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-1,1,ACCEL_TORSO,Z,5,-2,2,ACCEL_HIP,X,0,-3,3,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|ABORT_ANGLE1=40|MAX_THRUST2=7|MAX_REVERSE_THRUST2=6|MAX_VELOCITY2=30|MAX_ACCEL2=0.5|FARTHEST_FROM_TARGET|NO_MOTION_NO_THRUST)  
5,MotorAccel(MOTOR_LEFT_KNEE,8,2000,ACCEL_LEFT_UPPER_LEG,X,LULX0,-1,1,ACCEL_TORSO,Z,5,-2,2,ACCEL_HIP,X,0,-3,3,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|MAX_THRUST2=7|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,2000,ACCEL_LEFT_FOOT,X,0,-5,5,ACCEL_LEFT_LOWER_LEG,X,LLLX0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.5:2|MAX_THRUST=12|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20|OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET)
5,MotorAccel(MOTOR_RIGHT_FOOT,10,2000,ACCEL_RIGHT_FOOT,X,0,-5,5,ACCEL_RIGHT_LOWER_LEG,X,RLLX0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.5:2|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
5,MotorAccel(MOTOR_TORSO,3,2000,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.4:0.2|MAX_THRUST=6|MAX_REVERSE_THRUST=6|MIN_VELOCITY=5|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


#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
6,MotorAccel(MOTOR_RIGHT_SIDE,4,2000,ACCEL_RIGHT_UPPER_LEG,Z,3,-2,10,ACCEL_TORSO,Z,3,-1,5,WAIT_STAGE|MAX_THRUST0=4|MAX_REVERSE_THRUST0=4|MIN_VELOCITY0=5|MAX_VELOCITY0=10|MAX_ACCEL0=1|MAX_THRUST1=13|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,2000,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,2000,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,2000,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,2000,ACCEL_HIP,X,0,-1,1,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=6|MAX_REVERSE_THRUST=5|MAX_VELOCITY=40|MAX_ACCEL=1)
6,MotorAccel(MOTOR_RIGHT_LEG,3,2000,ACCEL_HIP,X,0,-1,1,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|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,2000,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,2000,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,2000,ACCEL_HIP,X,0,-3,3,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|FARTHEST_FROM_TARGET|NO_MOTION_NO_THRUST)  
6,MotorAccel(MOTOR_LEFT_KNEE,8,2000,ACCEL_HIP,X,0,-3,3,WAIT_STAGE|HOLD_ANGLE=0.5:0.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,2000,ACCEL_LEFT_FOOT,Z,0,-5,5,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=40)
#6,MotorAccel(MOTOR_RIGHT_ANKLE,3,2000,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,2000,ACCEL_RIGHT_UPPER_LEG,Z,3,-2,2,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=40|FUTURE_LOG)
#feet
6,MotorAccel(MOTOR_LEFT_FOOT,10,2000,ACCEL_LEFT_FOOT,X,0,-5,5,ACCEL_LEFT_LOWER_LEG,X,LLLX0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.5:2|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,2000,ACCEL_RIGHT_FOOT,X,0,-5,5,ACCEL_RIGHT_LOWER_LEG,X,RLLX0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.5:2|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,2000,ACCEL_TORSO,X,TX0,-1,1,WAIT_STAGE|HOLD_ANGLE=0.2:0.2|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 15, but bending knee causes 14 to happen quickly sometimes, same for lower left leg
7,MotorAccel(MOTOR_LEFT_LEG,14,400,ACCEL_LEFT_UPPER_LEG,X,18,-1,5,WAIT_STAGE|CONSTANT_THRUST)
7,MotorAccel(MOTOR_RIGHT_LEG,13,400,ACCEL_LEFT_UPPER_LEG,X,18,-1,5,WAIT_STAGE|CONSTANT_THRUST|HOLD_ANGLE)
7,MotorAccel(MOTOR_LEFT_KNEE,20,400,ACCEL_LEFT_LOWER_LEG,X,-30,-1,1,WAIT_STAGE|CONSTANT_THRUST|HOLD_ANGLE)
7,MotorAccel(MOTOR_RIGHT_KNEE,3,400,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=9|MAX_REVERSE_THRUST=6|MAINTAIN_THRUST|ABORT_ANGLE0=25)
7,MotorAccel(MOTOR_RIGHT_ANKLE,12,400,ACCEL_RIGHT_FOOT,Z,0,-15,20,ACCEL_RIGHT_LOWER_LEG,Z,7,-1,1,WAIT_STAGE|HOLD_ANGLE=2.0:10.0|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MAX_VELOCITY=5|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|HOLD_ANGLE|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MAX_VELOCITY=5|MAX_ACCEL=0.5)
#use HIP Z?
7,MotorAccel(MOTOR_RIGHT_SIDE,12,400,ACCEL_RIGHT_LOWER_LEG,Z,7,-7,2,ACCEL_TORSO,Z,8,-1,1,WAIT_STAGE|HOLD_ANGLE=0:40|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MIN_VELOCITY0=5|MAX_VELOCITY0=10|MAX_ACCEL0=1|MIN_VELOCITY1=20|MAX_VELOCITY1=50|MAX_ACCEL1=5.0|MAINTAIN_THRUST|NO_STOP_ON_TARGET|FUTURE_LOG)
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|HOLD_ANGLE=2:2|ABORT_ANGLE1=-30|MAX_THRUST=16|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20)
7,MotorAccel(MOTOR_LEFT_FOOT,10,400,ACCEL_LEFT_FOOT,X,0,-5,5,WAIT_STAGE|HOLD_ANGLE|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|HOLD_ANGLE=1:1|MAX_THRUST=6|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 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,3.0,250,500)
8,TouchSensor(TOUCH_LEFT_FOOT_BACK,GT,3.0,250,500)
8,MotorAccel(MOTOR_LEFT_LEG,12,500,ACCEL_LEFT_UPPER_LEG,X,LULX0,-1,1,WAIT_STAGE|CONSTANT_THRUST)
8,MotorAccel(MOTOR_RIGHT_LEG,12,500,ACCEL_LEFT_UPPER_LEG,X,LULX0,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=18|MAX_REVERSE_THRUST=3|CONSTANT_THRUST)
8,MotorAccel(MOTOR_LEFT_KNEE,20,500,ACCEL_LEFT_LOWER_LEG,X,-18,-1,1,ACCEL_LEFT_UPPER_LEG,X,0,0,0,WAIT_STAGE|HOLD_ANGLE|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,500,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)
8,MotorAccel(MOTOR_RIGHT_SIDE,6,500,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|MAX_REVERSE_THRUST=2|MAINTAIN_THRUST|RELATIVE_ANGLE0|FUTURE_LOG)
8,MotorAccel(MOTOR_LEFT_SIDE,10,500,ACCEL_LEFT_UPPER_LEG,Z,6,-1,1,ACCEL_RIGHT_UPPER_LEG,Z,0,0,0,WAIT_STAGE|HOLD_ANGLE=2:10|MAX_THRUST=10|MAX_REVERSE_THRUST=2|MAX_ACCEL=1|RELATIVE_ANGLE0)
8,MotorAccel(MOTOR_RIGHT_FOOT,10,500,ACCEL_RIGHT_FOOT,X,0,-10,10,ACCEL_RIGHT_LOWER_LEG,X,RLLX0,-1,1,WAIT_STAGE|HOLD_ANGLE=2:2|MAX_THRUST=16|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20)
8,MotorAccel(MOTOR_RIGHT_KNEE,10,500,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-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)
8,MotorAccel(MOTOR_LEFT_ANKLE,10,500,ACCEL_LEFT_FOOT,Z,0,-2,2,WAIT_STAGE|HOLD_ANGLE=1:1|MAX_THRUST=18|MAX_REVERSE_THRUST=6)
8,MotorAccel(MOTOR_LEFT_FOOT,10,500,ACCEL_LEFT_FOOT,X,0,-2,2,WAIT_STAGE|HOLD_ANGLE=1:1|MAX_THRUST=12|MAX_REVERSE_THRUST=6)
8,MotorAccel(MOTOR_TORSO,3,500,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE=1:1|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)
#Side motors in stage 9 need to keep upper leg Zs at a relative distance in order to stop the back and forth Z motion from the last step
9,MotorAccel(MOTOR_LEFT_SIDE,3,5000,ACCEL_LEFT_UPPER_LEG,Z,LULZ0,-3,3,ACCEL_HIP,Z,0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|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,-7,-1,1,ACCEL_LEFT_UPPER_LEG,Z,0,0,0,ACCEL_HIP,Z,0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=7|MAX_REVERSE_THRUST=3|MIN_VELOCITY=10|MAX_VELOCITY=15|MAX_ACCEL=1|FARTHEST_FROM_TARGET|NO_MOTION_NO_THRUST|RELATIVE_ANGLE0)
#9,MotorAccel(MOTOR_RIGHT_SIDE,3,5000,ACCEL_RIGHT_UPPER_LEG,Z,RULZ0,-3,3,ACCEL_HIP,Z,0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|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,3,5000,ACCEL_HIP,X,0,-1,1,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=6|MAX_REVERSE_THRUST=3|MAX_VELOCITY=40|MAX_ACCEL=1)
9,MotorAccel(MOTOR_RIGHT_LEG,3,5000,ACCEL_HIP,X,0,-1,1,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=6|MAX_REVERSE_THRUST=3|MAX_VELOCITY=40|MAX_ACCEL=1)
9,MotorAccel(MOTOR_RIGHT_KNEE,8,5000,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-2,2,ACCEL_HIP,Z,0,-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_VELOCITY=10|MAX_ACCEL=0.5|ABORT_ANGLE0=40|FARTHEST_FROM_TARGET|NO_MOTION_NO_THRUST)  
9,MotorAccel(MOTOR_LEFT_KNEE,8,5000,ACCEL_LEFT_UPPER_LEG,X,LULX0,-2,2,ACCEL_HIP,Z,0,-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)
#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|HOLD_ANGLE=1:2|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|HOLD_ANGLE=1:2|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
9,MotorAccel(MOTOR_LEFT_FOOT,10,5000,ACCEL_LEFT_FOOT,X,0,-10,10,ACCEL_LEFT_LOWER_LEG,X,LLLX0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.25:1|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,-10,10,ACCEL_RIGHT_LOWER_LEG,X,RLLX0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.25:1|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|HOLD_ANGLE=0.5:0.5|MAX_THRUST=5|MAX_REVERSE_THRUST=2|MIN_VELOCITY=10|MAX_VELOCITY=20|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

