#Right2StepUpDown03.sh
#Calibrated for LEG Z = +-3
#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,-3.0,-2,1,ACCEL_LEFT_UPPER_LEG,Z,-3.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,-3.0,-2,1,ACCEL_LEFT_UPPER_LEG,Z,-3.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 help but only to LULZ=0
#below inst skips attempt at reversing if not in range
#Torso Z should be pretty far to make sure ankle does not lift up
2,MotorAccel(MOTOR_LEFT_SIDE,16,2000,ACCEL_TORSO,Z,-3,-1,1,WAIT_STAGE|MAX_THRUST=16|MAX_REVERSE_THRUST=4|MIN_VELOCITY=30|MAX_VELOCITY=50|MAX_ACCEL=5|FUTURE_LOG|NO_STOP_ON_TARGET|MAINTAIN_THRUST|ABORT_ON_TIMEOUT|REQUIRED)
##2,MotorAccel(MOTOR_LEFT_SIDE,11,2000,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, NO_STOP_ON_TARGET|
#THRUST=15 can make foot turn > 10 degrees quickly, the torso should be doing most of the work in stage 2 I think
#What is perhaps really needed is a way to just lower thrust if LFZ>5 not necessary strive to put LFZ back
#only difference between A0 and A1 is MAX_THRUST
#try to keep LLLZ<-5 alt: try to keep LULZ and LLLZ to within 4deg of each other
2,MotorAccel(MOTOR_LEFT_ANKLE,3,2000,ACCEL_LEFT_UPPER_LEG,Z,-4.0,-0.5,1,ACCEL_LEFT_UPPER_LEG,Z,-4,-0.5,0.5,ACCEL_LEFT_FOOT,Z,0,-5,5,ACCEL_LEFT_LOWER_LEG,Z,0,-5,5,WAIT_STAGE|MAX_THRUST0=12|MAX_THRUST1=5|MAX_REVERSE_THRUST=6|MIN_VELOCITY=5|MAX_VELOCITY=8|ABORT_ANGLE0=30|TVR=15:1:3|MAX_ACCEL=2|MAINTAIN_THRUST|IF_A2_AND_A3|FUTURE_LOG)
#2,MotorAccel(MOTOR_LEFT_ANKLE,3,2000,ACCEL_LEFT_FOOT,Z,0,-5,5,ACCEL_LEFT_UPPER_LEG,Z,-4.5,-0.5,0.5,WAIT_STAGE|MAX_THRUST=8|MAX_REVERSE_THRUST=6|MIN_VELOCITY=10|MAX_VELOCITY=15|ABORT_ANGLE0=30|TVR=15:1:3|MAX_ACCEL=1|MAINTAIN_THRUST|FUTURE_LOG)
#2,MotorAccel(MOTOR_LEFT_ANKLE,3,2000,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,2000,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) |NO_STOP_ON_TARGET
2,MotorAccel(MOTOR_RIGHT_ANKLE,4,2000,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)
2,MotorAccel(MOTOR_LEFT_KNEE,4,2000,ACCEL_LEFT_UPPER_LEG,X,LULX0,-2,2,WAIT_STAGE|MAX_THRUST=4|MAX_REVERSE_THRUST=3|TVR=1:1:2|MAINTAIN_THRUST|ABORT_ANGLE0=25)
#Legs may need to push Hip X back vertically a little
2,MotorAccel(MOTOR_LEFT_LEG,4,2000,ACCEL_HIP,X,0,-2,2,WAIT_STAGE|TVR=5:20:4|MAX_THRUST=4|MAX_REVERSE_THRUST=3|MIN_VELOCITY=5|MAX_VELOCITY=8|MAX_ACCEL1=0.5)
2,MotorAccel(MOTOR_RIGHT_LEG,4,2000,ACCEL_HIP,X,0,-2,2,WAIT_STAGE|TVR=5:20:4|MAX_THRUST=4|MAX_REVERSE_THRUST=3|MIN_VELOCITY=5|MAX_VELOCITY=8|MAX_ACCEL1=0.5)

#|NO_STOP_ON_TARGET
#feet hold up lower legs
2,MotorAccel(MOTOR_LEFT_FOOT,3,2000,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,2000,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,2,2000,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|TVR=0.2:0.5:2|MAX_THRUST=5|MAX_REVERSE_THRUST=3|MAINTAIN_THRUST|MIN_VELOCITY=5|MAX_VELOCITY=10|MAX_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 was 21 but was maybe too big of a step for initially getting walking working
3,MotorAccel(MOTOR_RIGHT_LEG,14,400,ACCEL_RIGHT_UPPER_LEG,X,19,-1,5,WAIT_STAGE|CONSTANT_THRUST|REQUIRED)
3,MotorAccel(MOTOR_LEFT_LEG,14,400,ACCEL_RIGHT_UPPER_LEG,X,19,-1,5,WAIT_STAGE|MAINTAIN_THRUST|CONSTANT_THRUST)
#bend right knee so foot lifts in place - decrease angle X (lower leg rotates forward)
#14=too much momentum into stage 4
#Max_thrust=20 can cause EthMotors to crash but 19 is fine
3,MotorAccel(MOTOR_RIGHT_KNEE,17,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
#MAX THRUST was 20 but Left Ankle is lifting off ground, set FOOTZ max to 8
#only different between A0 and A1 is MAXTHRUST
#todo: Add protection in case the LLLZ<-5
#it seems important that the foot Z should stay firmly on the ground with no more than +-2degrees (most likely +2deg), and that the MaxThrust to adjust it should be small- because if moving >2 degrees the balance point has been lost, and thrusting more will only turn the foot Z more. 
3,MotorAccel(MOTOR_LEFT_ANKLE,12,400,ACCEL_LEFT_UPPER_LEG,Z,-2.5,-0.5,0.5,ACCEL_LEFT_LOWER_LEG,Z,0,-5,5,ACCEL_LEFT_FOOT,Z,0,-3,3,WAIT_STAGE|TVR=10.0:3.0:8|MAX_THRUST0=12|MAX_THRUST1=5|MAX_REVERSE_THRUST=7|MIN_VELOCITY=5|MAX_VELOCITY=8|MAX_ACCEL=3|ABORT_ANGLE0=30|OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET|MAINTAIN_THRUST|IF_NOT_A1_MAXTHRUST=8|IF_NOT_A2_MAXTHRUST=5|FUTURE_LOG)
#3,MotorAccel(MOTOR_LEFT_ANKLE,15,400,ACCEL_LEFT_FOOT,Z,0,-10,15,ACCEL_LEFT_LOWER_LEG,Z,-6,-0.5,0.5,ACCEL_LEFT_UPPER_LEG,Z,-4,-0.5,0.5,WAIT_STAGE|TVR=20.0:3.0:8|MAX_THRUST=10|MAX_REVERSE_THRUST=7|MIN_VELOCITY=5|MAX_VELOCITY=20|MAX_ACCEL=0.5|ABORT_ANGLE0=30|OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET|MAINTAIN_THRUST|FUTURE_LOG)
#3,MotorAccel(MOTOR_LEFT_ANKLE,20,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:8|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)
#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_LEFT_LOWER_LEG,Z,-6,-2,1,ACCEL_RIGHT_FOOT,Z,0,-3,3,WAIT_STAGE|MAX_THRUST=10|MAX_REVERSE_THRUST=2|MAX_VELOCITY=5|MAX_ACCEL=0.5|IF_NOT_A1_MAXTHRUST=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
#Torso Z should be far to make sure ankle does not lift- if ankle turns > 5 degrees, torso Z is not far enough
3,MotorAccel(MOTOR_LEFT_SIDE,15,400,ACCEL_TORSO,Z,-7,-1,1,WAIT_STAGE|TVR=10:0:8|MAX_THRUST=15|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
#was -7 but is -4 now (-2 is actual)- changed to 3+3=6+1-7
3,MotorAccel(MOTOR_RIGHT_SIDE,13,400,ACCEL_RIGHT_UPPER_LEG,Z,-7,-1,1,ACCEL_LEFT_UPPER_LEG,Z,0,0,0,WAIT_STAGE|TVR=20:0:8|MAX_THRUST=13|MAX_REVERSE_THRUST=2|MIN_VELOCITY=20|MAX_VELOCITY=30|MAX_ACCEL=3.0|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
#OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET|
3,MotorAccel(MOTOR_TORSO,3,400,ACCEL_TORSO,X,TX0,-1,1,WAIT_STAGE|TVR=0.5:4:3|MAX_THRUST=11|MAX_REVERSE_THRUST=3|MIN_VELOCITY=20|MAX_VELOCITY=25|MAX_ACCEL=10|NO_MOTION_NO_THRUST|NO_STOP_ON_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,30.0,250,1000)
4,TouchSensor(TOUCH_RIGHT_FOOT_BACK,GT,30.0,250,1000)
#needs MIN_DUR=200 (ms) because right leg can sometimes still be going up and be around 20 degrees
#with requiring VELOCITY_A3>-10 flag, range of RULX can be anything, until V<=-10 so instead of 19 to 19+75, just use 0-75
4,MotorAccel(MOTOR_LEFT_ANKLE,10,1000,ACCEL_LEFT_UPPER_LEG,Z,-2.5,-0.5,0.5,ACCEL_LEFT_UPPER_LEG,Z,2,-1,20,ACCEL_RIGHT_UPPER_LEG,Z,2,-20,0,ACCEL_RIGHT_UPPER_LEG,X,22,0,75,ACCEL_LEFT_FOOT,Z,0,-3,3,WAIT_STAGE|MAX_THRUST0=14|MAX_THRUST1=5|MAX_REVERSE_THRUST=5|MAX_ACCEL0=1|MIN_VELOCITY0=10|MAX_VELOCITY0=20|MAX_ACCEL1=1|MIN_VELOCITY1=10|MAX_VELOCITY1=20|FUTURE_LOG|ABORT_ANGLE2=30|TVR0=20.0:3.0:8|IF_A2_AND_A3|IF_NOT_A4_MAXTHRUST=5|VELOCITY_A3_OR>-20|NO_STOP_ON_TARGET|MAINTAIN_THRUST|ABORT_ON_TIMEOUT|REQUIRED)
#4,MotorAccel(MOTOR_LEFT_ANKLE,10,1000,ACCEL_LEFT_UPPER_LEG,Z,-2.5,-0.5,0.5,ACCEL_LEFT_UPPER_LEG,Z,2,-1,20,ACCEL_RIGHT_UPPER_LEG,Z,2,-20,0,ACCEL_RIGHT_UPPER_LEG,X,20,0,75,ACCEL_LEFT_FOOT,Z,0,-5,5,WAIT_STAGE|MAX_THRUST0=14|MAX_THRUST1=5|MAX_REVERSE_THRUST=5|MIN_VELOCITY0=10|MAX_VELOCITY0=20|MAX_ACCEL0=1|MIN_VELOCITY1=10|MAX_VELOCITY1=20|MAX_ACCEL1=1|FUTURE_LOG|ABORT_ANGLE2=30|TVR0=20.0:3.0:8|IF_A2_AND_A3|IF_NOT_A4_MAXTHRUST=5|NO_STOP_ON_TARGET|MAINTAIN_THRUST|ABORT_ON_TIMEOUT|REQUIRED)
#4,MotorAccel(MOTOR_LEFT_ANKLE,10,1000,ACCEL_LEFT_UPPER_LEG,Z,-3,-0.5,0.5,ACCEL_LEFT_UPPER_LEG,Z,2,-1,20,ACCEL_RIGHT_UPPER_LEG,Z,2,-20,0,ACCEL_LEFT_FOOT,Z,0,-5,5,WAIT_STAGE|MAX_THRUST0=14|MAX_THRUST1=5|MAX_REVERSE_THRUST=5|MIN_VELOCITY0=10|MAX_VELOCITY0=20|MAX_ACCEL0=1|MIN_VELOCITY1=10|MAX_VELOCITY1=20|MAX_ACCEL1=1|FUTURE_LOG|ABORT_ANGLE2=30|TVR0=20.0:3.0:8|IF_A2_AND_A3|NO_STOP_ON_TARGET|MAINTAIN_THRUST|ABORT_ON_TIMEOUT)
#4,MotorAccel(MOTOR_LEFT_ANKLE,10,1000,ACCEL_LEFT_UPPER_LEG,Z,-3,-0.5,0.5,ACCEL_LEFT_UPPER_LEG,Z,2,-1,20,ACCEL_RIGHT_UPPER_LEG,Z,2,-20,0,ACCEL_RIGHT_UPPER_LEG,X,19,0,75,WAIT_STAGE|MAX_THRUST0=14|MAX_THRUST1=5|MAX_REVERSE_THRUST=5|MIN_VELOCITY0=10|MAX_VELOCITY0=20|MAX_ACCEL0=1|MIN_VELOCITY1=10|MAX_VELOCITY1=20|MAX_ACCEL1=1|FUTURE_LOG|ABORT_ANGLE2=30|TVR0=20.0:3.0:8|IF_A2_AND_A3|NO_STOP_ON_TARGET|MAINTAIN_THRUST|ABORT_ON_TIMEOUT|REQUIRED)
4,MotorAccel(MOTOR_RIGHT_LEG,8,1000,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-2,2,WAIT_STAGE|MAX_ACCEL=1|MIN_VELOCITY=10|MAX_VELOCITY=40|MAX_THRUST=9)
#4,MotorAccel(MOTOR_RIGHT_LEG,8,1000,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-2,2,WAIT_STAGE|CONSTANT_THRUST)
4,MotorAccel(MOTOR_LEFT_LEG,8,1000,ACCEL_HIP,X,0,-1,1,WAIT_STAGE|MAX_THRUST=18|MAX_REVERSE_THRUST=3|TVR=0.3:4:4)
4,MotorAccel(MOTOR_RIGHT_KNEE,12,1000,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=12|MAINTAIN_THRUST)
4,MotorAccel(MOTOR_LEFT_SIDE,15,1000,ACCEL_TORSO,Z,-6,-1,1,ACCEL_TORSO,Z,6,-1,1,ACCEL_RIGHT_UPPER_LEG,Z,2,-20,0,ACCEL_RIGHT_UPPER_LEG,X,22,0,75,WAIT_STAGE|TVR0=20:4:8|MAX_THRUST0=15|MAX_THRUST1=8|MAX_REVERSE_THRUST=2|MAX_ACCEL0=10|MIN_VELOCITY0=20|MAX_VELOCITY0=30|MAX_ACCEL1=10|MIN_VELOCITY1=10|MAX_VELOCITY1=20|MAINTAIN_THRUST|IF_A2_AND_A3|VELOCITY_A3_OR>-10|FUTURE_LOG)
4,MotorAccel(MOTOR_RIGHT_SIDE,13,1000,ACCEL_RIGHT_UPPER_LEG,Z,-7,-1,1,ACCEL_LEFT_UPPER_LEG,Z,0,0,0,WAIT_STAGE|TVR=10:1:8|MAX_THRUST=13|MAX_REVERSE_THRUST=2|MAX_ACCEL=3|MIN_VELOCITY=20|MAX_VELOCITY=30|RELATIVE_ANGLE0|FUTURE_LOG)
4,MotorAccel(MOTOR_LEFT_FOOT,10,1000,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,14,1000,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=14|MAX_REVERSE_THRUST=6|ABORT_ANGLE0=30|RELATIVE_ANGLE0)
4,MotorAccel(MOTOR_RIGHT_ANKLE,10,1000,ACCEL_RIGHT_FOOT,Z,0,-3,3,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_FOOT,10,1000,ACCEL_RIGHT_FOOT,X,0,-2,2,WAIT_STAGE|TVR=0:1:5|MAX_THRUST=9|MAX_REVERSE_THRUST=6)
#OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET| |NO_STOP_ON_TARGET
4,MotorAccel(MOTOR_TORSO,3,1000,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|TVR=0.5:4:3|MAX_THRUST=12|MAX_REVERSE_THRUST=2|MIN_VELOCITY=10|MAX_VELOCITY=20|MAX_ACCEL=10|MAINTAIN_THRUST|FUTURE_LOG)


#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,
#seems most likely that the RAM needs to brake the entire way. Need to let RULZ go at least to +8  
5,MotorAccel(MOTOR_RIGHT_ANKLE,9,1500,ACCEL_RIGHT_UPPER_LEG,Z,2,-1,6,ACCEL_RIGHT_FOOT,Z,0,-3,3,WAIT_STAGE|MAX_THRUST0=14|MAX_THRUST1=5|MAX_REVERSE_THRUST=6|MAX_ACCEL=0.5|MIN_VELOCITY=5|MAX_VELOCITY=10|MAINTAIN_THRUST|ABORT_ANGLE1=30|ABORT_ON_TIMEOUT|IF_NOT_A1_MAXTHRUST=5|FUTURE_LOG|REQUIRED)
#5,MotorAccel(MOTOR_RIGHT_ANKLE,9,1500,ACCEL_RIGHT_FOOT,Z,0,-8,15,ACCEL_RIGHT_UPPER_LEG,Z,1,-1,6,WAIT_STAGE|MAX_THRUST=9|MAX_REVERSE_THRUST=6|MAX_ACCEL=5|MIN_VELOCITY=20|MAX_VELOCITY=30|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
#NO_STOP_ON_TARGET|  
#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|
#KNEE IS ALSO REQUIRED- stage needs to wait for RUL to straighten  
5,MotorAccel(MOTOR_RIGHT_KNEE,14,1500,ACCEL_RIGHT_UPPER_LEG,X,24,-3,3,ACCEL_RIGHT_LOWER_LEG,X,0,0,0,WAIT_STAGE|TVR=2:0.5:5|MAX_THRUST0=14|MAX_REVERSE_THRUST0=6|MAX_ACCEL0=10|MIN_VELOCITY0=20|MAX_VELOCITY0=40|ABORT_ANGLE0=40|MAINTAIN_THRUST|NO_MOTION_NO_THRUST|RELATIVE_ANGLE0|REQUIRED|FUTURE_LOG)
#5,MotorAccel(MOTOR_RIGHT_KNEE,14,1500,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-2,2,WAIT_STAGE|TVR=2:0.5:5|MAX_THRUST0=14|MAX_REVERSE_THRUST0=6|MAX_ACCEL0=10|MIN_VELOCITY0=20|MAX_VELOCITY0=40|ABORT_ANGLE0=40|MAINTAIN_THRUST|NO_MOTION_NO_THRUST|REQUIRED|FUTURE_LOG)
#5,MotorAccel(MOTOR_RIGHT_KNEE,14,1500,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-2,2,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|MAX_ACCEL0=10|MIN_VELOCITY0=20|MAX_VELOCITY0=40|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|MAINTAIN_THRUST|NO_MOTION_NO_THRUST|REQUIRED|FUTURE_LOG)
#
#5,MotorAccel(MOTOR_LEFT_KNEE,10,1500,ACCEL_LEFT_UPPER_LEG,X,LULX0,-6,1,WAIT_STAGE|TVR=1:0.5:5|MAX_THRUST0=10|MAX_REVERSE_THRUST0=6|MIN_VELOCITY0=20|MAX_VELOCITY0=30|MAX_ACCEL0=10|FUTURE_LOG)
5,MotorAccel(MOTOR_LEFT_KNEE,10,1500,ACCEL_LEFT_UPPER_LEG,X,LULX0,-6,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)
#velocity of RULZ is important to limit
5,MotorAccel(MOTOR_LEFT_ANKLE,8,1500,ACCEL_RIGHT_UPPER_LEG,Z,2,-1,8,ACCEL_RIGHT_LOWER_LEG,Z,4,-3,8,ACCEL_LEFT_FOOT,Z,0,-3,3,WAIT_STAGE|MAX_THRUST=8|MAX_REVERSE_THRUST=5|MAX_ACCEL=1|MIN_VELOCITY=10|MAX_VELOCITY=20|MAINTAIN_THRUST|IF_NOT_A2_MAXTHRUST=3|FUTURE_LOG|ABORT_ANGLE0=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,4,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 
#LSM can largely help lean robot right if leaning just a little left at start of stage 5
5,MotorAccel(MOTOR_LEFT_SIDE,5,1500,ACCEL_RIGHT_UPPER_LEG,Z,3.5,-1,2,ACCEL_RIGHT_LOWER_LEG,Z,4.5,-1,2,WAIT_STAGE|MAX_ACCEL=1|MIN_VELOCITY=10|MAX_VELOCITY=20|MAX_THRUST=7|MAX_REVERSE_THRUST=2|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

#LKM can give a little push to the right (in conjunction with the LFM)  
#5,MotorAccel(MOTOR_LEFT_KNEE,10,1500,ACCEL_LEFT_UPPER_LEG,X,CurrentAnglePlus,-5,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
#left foot can give a little push to the right
#5,MotorAccel(MOTOR_LEFT_FOOT,10,1500,ACCEL_LEFT_FOOT,X,0,-5,5,ACCEL_LEFT_LOWER_LEG,X,CurrentAnglePlus,5,-1,1,WAIT_STAGE|TVR=0.5:2:5|MAX_THRUST=12|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20|OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET)
#weight is moving to right too much currently- so removed push from above inst
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:2:3|MAX_THRUST=9|MAX_REVERSE_THRUST=2|MIN_VELOCITY=10|MAX_VELOCITY=20|MAX_ACCEL=10|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
#Possibly require that diff between RULX and RLLX be  -24  (-12RULX - 12RLLX)=-24
#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)
#FARTHEST_FROM_TARGET|
6,MotorAccel(MOTOR_RIGHT_KNEE,14,1000,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-2,2,WAIT_STAGE|TVR=3:0.5:5|MAX_THRUST0=14|MAX_REVERSE_THRUST0=6|MAX_ACCEL=10|MIN_VELOCITY=20|MAX_VELOCITY0=40|ABORT_ANGLE0=40|NO_MOTION_NO_THRUST) 
#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,3,-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 changed to 19
7,MotorAccel(MOTOR_LEFT_LEG,14,400,ACCEL_LEFT_UPPER_LEG,X,19,-1,5,WAIT_STAGE|CONSTANT_THRUST|REQUIRED)
7,MotorAccel(MOTOR_RIGHT_LEG,14,400,ACCEL_LEFT_UPPER_LEG,X,19,-1,5,WAIT_STAGE|CONSTANT_THRUST)
#was -30 changed to -20
7,MotorAccel(MOTOR_LEFT_KNEE,16,400,ACCEL_LEFT_LOWER_LEG,X,-20,-1,1,WAIT_STAGE|CONSTANT_THRUST)
#TVR=0.5:0.5:3|
7,MotorAccel(MOTOR_RIGHT_KNEE,14,400,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-2,2,WAIT_STAGE|MAX_THRUST=14|MAX_ACCEL=5|MIN_VELOCITY=10|MAX_VELOCITY=30|MAX_REVERSE_THRUST=6|MAINTAIN_THRUST|ABORT_ANGLE0=25)

7,MotorAccel(MOTOR_RIGHT_ANKLE,12,400,ACCEL_RIGHT_UPPER_LEG,Z,2.0,-0.5,0.5,ACCEL_RIGHT_UPPER_LEG,Z,RULZ0,-0.5,0.5,ACCEL_RIGHT_LOWER_LEG,Z,0,-5,5,ACCEL_RIGHT_FOOT,Z,0,-5,5,WAIT_STAGE|TVR=3.0:20.0:8|MAX_THRUST0=12|MAX_THRUST1=10|MAX_REVERSE_THRUST=7|MIN_VELOCITY=5|MAX_VELOCITY=8|MAX_ACCEL=3|ABORT_ANGLE0=30|OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET|MAINTAIN_THRUST|IF_A2|IF_NOT_A3_MAXTHRUST=5|FUTURE_LOG)
#7,MotorAccel(MOTOR_RIGHT_ANKLE,14,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=14|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,15,400,ACCEL_TORSO,Z,6,-1,1,WAIT_STAGE|TVR=0:1:8|MAX_THRUST=15|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
#was 7 changed to 4 then to 7 ACCEL_LEFT_UPPER_LEG,X,21,-1,5,
7,MotorAccel(MOTOR_LEFT_SIDE,15,400,ACCEL_LEFT_UPPER_LEG,Z,7,-1,1,ACCEL_RIGHT_UPPER_LEG,Z,0,0,0,WAIT_STAGE|TVR=0:20:8|MAX_THRUST=15|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
#OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET| - ignores MAX_VELOCITY
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|MAINTAIN_THRUST|FUTURE_LOG)


#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,30.0,250,1000)
8,TouchSensor(TOUCH_LEFT_FOOT_BACK,GT,30.0,250,1000)
#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
#ACCEL_LEFT_UPPER_LEG,X,20,0,75,
8,MotorAccel(MOTOR_RIGHT_ANKLE,10,1000,ACCEL_RIGHT_UPPER_LEG,Z,3,-0.5,0.5,ACCEL_RIGHT_UPPER_LEG,Z,RULZ0,-1,20,ACCEL_LEFT_UPPER_LEG,Z,-2,0,20,ACCEL_RIGHT_FOOT,Z,0,-5,5,WAIT_STAGE|MAX_THRUST0=14|MAX_THRUST1=5|MAX_REVERSE_THRUST=5|MIN_VELOCITY0=10|MAX_VELOCITY0=20|MAX_ACCEL0=1|MIN_VELOCITY1=10|MAX_VELOCITY1=20|MAX_ACCEL1=1|FUTURE_LOG|ABORT_ANGLE2=30|TVR0=3.0:20.0:8|IF_A2|IF_NOT_A3_MAXTHRUST=5|NO_STOP_ON_TARGET|MAINTAIN_THRUST|ABORT_ON_TIMEOUT|REQUIRED)
#8,MotorAccel(MOTOR_RIGHT_ANKLE,3,1000,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,1000,ACCEL_LEFT_UPPER_LEG,X,LULX0,-1,1,WAIT_STAGE|CONSTANT_THRUST)
8,MotorAccel(MOTOR_RIGHT_LEG,12,1000,ACCEL_LEFT_UPPER_LEG,X,LULX0,-1,1,WAIT_STAGE|MAX_THRUST=16|MAX_REVERSE_THRUST=3|CONSTANT_THRUST)
8,MotorAccel(MOTOR_LEFT_KNEE,16,1000,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=16|CONSTANT_THRUST20:4)
##8,MotorAccel(MOTOR_RIGHT_ANKLE,3,1000,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)
#leg Z spacing was 7 changed to 4 back to 7
8,MotorAccel(MOTOR_RIGHT_SIDE,6,1000,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)
#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
#was 7 changed to 4 then to 7 ACCEL_LEFT_UPPER_LEG,Z,LULX0,-2,2,
8,MotorAccel(MOTOR_LEFT_SIDE,8,1000,ACCEL_LEFT_UPPER_LEG,Z,7,-1,1,ACCEL_RIGHT_UPPER_LEG,Z,0,0,0,WAIT_STAGE|TVR=1:10:6|MAX_THRUST=8|MAX_REVERSE_THRUST=2|MIN_VELOCITY=10|MAX_VELOCITY=30|RELATIVE_ANGLE0|MAX_ACCEL=3)
8,MotorAccel(MOTOR_RIGHT_FOOT,10,1000,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,1000,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-2,2,ACCEL_HIP,Z,0,-2,2,WAIT_STAGE|TVR=0.5:0.5:5|MAX_ACCEL=5|MIN_VELOCITY=10|MAX_VELOCITY=30|MAX_THRUST=12|MAX_REVERSE_THRUST=6|MAINTAIN_THRUST|ABORT_ANGLE0=30)
8,MotorAccel(MOTOR_LEFT_ANKLE,10,1000,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,1000,ACCEL_LEFT_FOOT,X,0,-2,2,WAIT_STAGE|TVR=1:1:5|MAX_THRUST=12|MAX_REVERSE_THRUST=6)
#OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET| - when in range moving to target, uses TVR but ignores MAX_VELOCITY, etc.
8,MotorAccel(MOTOR_TORSO,3,1000,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|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
#NO_MOTION_NO_THRUST0|
9,MotorAccel(MOTOR_LEFT_FOOT,10,5000,ACCEL_LEFT_LOWER_LEG,X,LLLX0,-2,2,ACCEL_LEFT_FOOT,X,0,-5,5,WAIT_STAGE|TVR=0.25:1:5|MAX_THRUST=11|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20|IF_NOT_A1_MAXTHRUST=3|OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET)
#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_LOWER_LEG,X,RLLX0,-2,2,ACCEL_RIGHT_FOOT,X,0,-5,5,WAIT_STAGE|TVR=0.25:1:5|MAX_THRUST=10|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20|NO_MOTION_NO_THRUST0|OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET)
#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|IF_NOT_A1_MAXTHRUST=3|FUTURE_LOG)
#possibly right leg motor needs to hold up leg or possibly falls down

12000,StopLogging

