#Right2StepUpDown05.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)
#do not |MAINTAIN_THRUST because is usually braking, and target range changes in stage 2
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)
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|MIN_VELOCITY=5|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
#no MAINTAIN_THRUST|because when in range (is REQUIRED), Thrust is set to 0, and so T=0 is maintained instead of an initial thrust
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|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,-3.5,-1,0.5,ACCEL_LEFT_UPPER_LEG,Z,-3.5,-1,0.5,ACCEL_LEFT_FOOT,Z,0,-5,5,ACCEL_LEFT_LOWER_LEG,Z,0,-8,5,WAIT_STAGE|MAX_THRUST0=14|MAX_THRUST1=5|MAX_REVERSE_THRUST=6|MAX_ACCEL=5|MIN_VELOCITY=10|MAX_VELOCITY=20|ABORT_ANGLE0=30|TVR=15:1:3|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.5,-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,20,-1,5,WAIT_STAGE|CONSTANT_THRUST|REQUIRED)
3,MotorAccel(MOTOR_LEFT_LEG,14,400,ACCEL_RIGHT_UPPER_LEG,X,20,-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, 17 caused EthMotors crash
3,MotorAccel(MOTOR_RIGHT_KNEE,16,400,ACCEL_RIGHT_LOWER_LEG,X,-20,-1,1,WAIT_STAGE|CONSTANT_THRUST)
#HOLD_ANGLE=1:0.5
3,MotorAccel(MOTOR_LEFT_KNEE,14,400,ACCEL_LEFT_UPPER_LEG,X,LULX0,-2,2,WAIT_STAGE|MAX_THRUST=14|MAX_REVERSE_THRUST=14|MAX_ACCEL=1|MIN_VELOCITY=10|MAX_VELOCITY=20|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,-1.5,0.5,ACCEL_LEFT_LOWER_LEG,Z,0,-7,5,ACCEL_LEFT_FOOT,Z,0,-3,3,WAIT_STAGE|TVR=10.0:3.0:8|MAX_THRUST0=14|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=9|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_RIGHT_FOOT,Z,0,-3,3,ACCEL_LEFT_LOWER_LEG,Z,-6,-2,1,WAIT_STAGE|MAX_THRUST=10|MAX_REVERSE_THRUST=2|MAX_VELOCITY=5|MAX_ACCEL=0.5)
#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,14,400,ACCEL_TORSO,Z,-7,-1,1,WAIT_STAGE|TVR=10:0:8|MAX_THRUST=14|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:5:3|MAX_THRUST=4|MAX_REVERSE_THRUST=3|MIN_VELOCITY=5|MAX_VELOCITY=10|MAX_ACCEL=5|NO_MOTION_NO_THRUST|NO_STOP_ON_TARGET|MAINTAIN_THRUST)


#Stage 4: LEFT ANKLE AND SIDE MOTORS CONTINUE TO LEAN LEFT, RIGHT LOWER LEG X SWINGS FORWARD, RIGHT UPPER LEG X FINISHES ASCENDING. STAGE ENDS WHEN UPPER RIGHT LEG X STARTS TO FALL WITH V<-10 OR RIGHT UPPER LEG Z IS >0
#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
#ACCEL_RIGHT_UPPER_LEG Z was 2,-20,0 changed to 0- ending stage 4 sooner is better to stop fast fall to right
#ACCEL_RIGHT_UPPER_LEG,Z,0,-20,0,
4,MotorAccel(MOTOR_LEFT_ANKLE,10,1000,ACCEL_LEFT_UPPER_LEG,Z,-2.5,-0.5,0.5,ACCEL_RIGHT_UPPER_LEG,X,22,0,75,ACCEL_LEFT_FOOT,Z,0,-5,5,WAIT_STAGE|MAX_THRUST0=14|MAX_REVERSE_THRUST=5|MAX_ACCEL0=1|MIN_VELOCITY0=10|MAX_VELOCITY0=20|ABORT_ANGLE2=30|TVR0=20.0:3.0:8|IF_A1|IF_NOT_A2_MAXTHRUST=5|VELOCITY_A1_OR>-10|NO_STOP_ON_TARGET|MAINTAIN_THRUST|ABORT_ON_TIMEOUT|REQUIRED|FUTURE_LOG)
#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,0,-20,0,ACCEL_RIGHT_UPPER_LEG,X,22,0,75,ACCEL_LEFT_FOOT,Z,0,-5,5,WAIT_STAGE|MAX_THRUST0=14|MAX_REVERSE_THRUST=5|MAX_ACCEL0=1|MIN_VELOCITY0=10|MAX_VELOCITY0=20|MAX_THRUST1=10|MAX_ACCEL1=1|MIN_VELOCITY1=20|MAX_VELOCITY1=30|FUTURE_LOG|ABORT_ANGLE2=30|TVR0=20.0:3.0:8|TVR1=0.5:20.0:8|IF_A2_AND_A3|IF_NOT_A4_MAXTHRUST=5|VELOCITY_A3_OR>-10|NO_STOP_ON_TARGET|MAINTAIN_THRUST|ABORT_ON_TIMEOUT|REQUIRED)
#
4,MotorAccel(MOTOR_RIGHT_LEG,12,1000,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-2,2,WAIT_STAGE|MAX_ACCEL=1|MIN_VELOCITY=10|MAX_VELOCITY=40|MAX_THRUST=12)
#4,MotorAccel(MOTOR_RIGHT_LEG,8,1000,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-2,2,WAIT_STAGE|CONSTANT_THRUST)
4,MotorAccel(MOTOR_LEFT_LEG,12,1000,ACCEL_HIP,X,0,-1,1,WAIT_STAGE|MAX_THRUST=12|MAX_REVERSE_THRUST=3|TVR=0.3:4:4)
4,MotorAccel(MOTOR_RIGHT_KNEE,14,1000,ACCEL_RIGHT_UPPER_LEG,X,24,-1,1,ACCEL_RIGHT_LOWER_LEG,X,0,0,0,WAIT_STAGE|RELATIVE_ANGLE0|ABORT_ANGLE0=-40|MAX_ACCEL=14|MIN_VELOCITY=30|MAX_VELOCITY=50|MAX_THRUST=12|MAINTAIN_THRUST)
#need OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET because otherwise thrust does not go high enough quickly enough
4,MotorAccel(MOTOR_LEFT_SIDE,14,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=14|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|OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET|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,1,-1,1,WAIT_STAGE|MAX_THRUST=7|MAX_REVERSE_THRUST=5)
#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=4|MAX_REVERSE_THRUST=2|MIN_VELOCITY=5|MAX_VELOCITY=10|MAX_ACCEL=5|MAINTAIN_THRUST|FUTURE_LOG)

#Stage 5: RIGHT LOWER LEG CONTINUES TO SWING FORWARD, RIGHT UPPER LEG IS NOW DESCENDING, LEFT ANKLE AND SIDE ARE NOW LEANING TO RIGHT. STAGE ENDS WHEN ANY RIGHT FOOT TOUCH SENSOR >30 OR LULZ<1
#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
#ACCEL_RIGHT_UPPER_LEG Z was 2,-20,0 changed to 0- ending stage 4 sooner is better to stop fast fall to right
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,0,-20,0,ACCEL_RIGHT_UPPER_LEG,X,22,0,75,ACCEL_LEFT_FOOT,Z,0,-5,5,WAIT_STAGE|MAX_THRUST0=14|MAX_REVERSE_THRUST=5|MAX_ACCEL0=1|MIN_VELOCITY0=10|MAX_VELOCITY0=20|MAX_THRUST1=10|MAX_ACCEL1=1|MIN_VELOCITY1=20|MAX_VELOCITY1=30|FUTURE_LOG|ABORT_ANGLE2=30|TVR0=20.0:3.0:8|TVR1=0.5:20.0:8|IF_A2_AND_A3|IF_NOT_A4_MAXTHRUST=5|VELOCITY_A3_OR>-10|NO_STOP_ON_TARGET|MAINTAIN_THRUST|ABORT_ON_TIMEOUT|REQUIRED)
#
4,MotorAccel(MOTOR_RIGHT_LEG,12,1000,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-2,2,WAIT_STAGE|MAX_ACCEL=1|MIN_VELOCITY=10|MAX_VELOCITY=40|MAX_THRUST=12)
#4,MotorAccel(MOTOR_RIGHT_LEG,8,1000,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-2,2,WAIT_STAGE|CONSTANT_THRUST)
4,MotorAccel(MOTOR_LEFT_LEG,12,1000,ACCEL_HIP,X,0,-1,1,WAIT_STAGE|MAX_THRUST=12|MAX_REVERSE_THRUST=3|TVR=0.3:4:4)
4,MotorAccel(MOTOR_RIGHT_KNEE,14,1000,ACCEL_RIGHT_UPPER_LEG,X,24,-1,1,ACCEL_RIGHT_LOWER_LEG,X,0,0,0,WAIT_STAGE|RELATIVE_ANGLE0|ABORT_ANGLE0=-40|MAX_ACCEL=14|MIN_VELOCITY=30|MAX_VELOCITY=50|MAX_THRUST=12|MAINTAIN_THRUST)
#need OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET because otherwise thrust does not go high enough quickly enough
4,MotorAccel(MOTOR_LEFT_SIDE,14,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=14|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|OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET|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,1,-1,1,WAIT_STAGE|MAX_THRUST=7|MAX_REVERSE_THRUST=5)
#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=4|MAX_REVERSE_THRUST=2|MIN_VELOCITY=5|MAX_VELOCITY=10|MAX_ACCEL=5|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
#ABORT_ON_TIMEOUT|
5,MotorAccel(MOTOR_RIGHT_ANKLE,9,1500,ACCEL_RIGHT_UPPER_LEG,Z,2,-1,7,ACCEL_RIGHT_FOOT,Z,0,-3,3,WAIT_STAGE|MAX_THRUST0=14|MAX_REVERSE_THRUST=6|MAX_ACCEL=10|MIN_VELOCITY=20|MAX_VELOCITY=30|TVR=1:1|OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET|MAINTAIN_THRUST|ABORT_ANGLE1=30|IF_NOT_A1_MAXTHRUST=5|FUTURE_LOG|REQUIRED)
#KNEE IS ALSO REQUIRED- stage needs to wait for RUL to straighten   NO_MOTION_NO_THRUST|
5,MotorAccel(MOTOR_RIGHT_KNEE,14,1500,ACCEL_RIGHT_UPPER_LEG,X,24,-4,4,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=30|MAX_VELOCITY0=40|ABORT_ANGLE0=40|MAINTAIN_THRUST|RELATIVE_ANGLE0|REQUIRED|FUTURE_LOG)
#
5,MotorAccel(MOTOR_LEFT_KNEE,10,1500,ACCEL_LEFT_UPPER_LEG,X,24,-3,3,ACCEL_LEFT_LOWER_LEG,X,0,0,0,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=2|MAX_THRUST1=14|MAX_REVERSE_THRUST1=6|MAX_ACCEL1=2|MIN_VELOCITY1=20|MAX_VELOCITY1=30|FARTHEST_FROM_TARGET|NO_MOTION_NO_THRUST|RELATIVE_ANGLE0|FUTURE_LOG)
#velocity of RULZ is important to limit
5,MotorAccel(MOTOR_LEFT_ANKLE,8,1500,ACCEL_RIGHT_UPPER_LEG,Z,3,-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
#RSM needs to help stop the fall to the right side
5,MotorAccel(MOTOR_RIGHT_SIDE,8,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=14|MAX_REVERSE_THRUST=2|MAX_ACCEL=1|TVR=4:0.5:8)
#LSM needs to assist in braking lean to right
5,MotorAccel(MOTOR_LEFT_SIDE,5,1500,ACCEL_RIGHT_UPPER_LEG,Z,3,-2,2,ACCEL_TORSO,Z,3,-2,2,WAIT_STAGE|MAX_ACCEL=1|MIN_VELOCITY=10|MAX_VELOCITY=20|MAX_THRUST=7|MAX_REVERSE_THRUST=2|ABORT_ON_TIMEOUT)
#5,MotorAccel(MOTOR_LEFT_SIDE,5,1500,ACCEL_TORSO,Z,3,-2,2,WAIT_STAGE|MAX_ACCEL=1|MIN_VELOCITY=10|MAX_VELOCITY=20|MAX_THRUST=7|MAX_REVERSE_THRUST=2|ABORT_ON_TIMEOUT)
#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)
#legs
5,MotorAccel(MOTOR_LEFT_LEG,3,1500,ACCEL_HIP,X,0,-1,1,WAIT_STAGE|TVR=1: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=1:1:3|MAX_THRUST=10|MAX_REVERSE_THRUST=3|MIN_VELOCITY=20|MAX_VELOCITY=40|MAX_ACCEL=1)
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)
#OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET|
5,MotorAccel(MOTOR_TORSO,3,1500,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|TVR=0.4:4:3|MAX_THRUST=4|MAX_REVERSE_THRUST=2|MAX_ACCEL=5|MIN_VELOCITY=2|MAX_VELOCITY=5|NO_MOTION_NO_THRUST|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||NO_MOTION_NO_THRUST
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) 
#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.5,-0.5,1,ACCEL_RIGHT_UPPER_LEG,Z,3.5,-0.5,0.5,ACCEL_RIGHT_FOOT,Z,0,-5,5,ACCEL_RIGHT_LOWER_LEG,Z,0,-5,5,WAIT_STAGE|MAX_THRUST0=14|MAX_THRUST1=5|MAX_REVERSE_THRUST=6|MIN_VELOCITY=5|MAX_VELOCITY=8|ABORT_ANGLE0=30|TVR=1:15:3|MAX_ACCEL=2|MAINTAIN_THRUST|IF_A2_AND_A3|FUTURE_LOG)
#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|MAINTAIN_THRUST|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
#OPPOSE_MOTION_IN_RANGE_MOVING_TO_TARGET|
6,MotorAccel(MOTOR_TORSO,3,1000,ACCEL_TORSO,X,TX0,-1,1,WAIT_STAGE|TVR=0.4:4:3|MAX_THRUST=4|MAX_REVERSE_THRUST=5|MAX_ACCEL=5|MIN_VELOCITY=5|MAX_VELOCITY=10|MAINTAIN_THRUST|NO_MOTION_NO_THRUST|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,20,-1,5,WAIT_STAGE|CONSTANT_THRUST|REQUIRED)
7,MotorAccel(MOTOR_RIGHT_LEG,14,400,ACCEL_LEFT_UPPER_LEG,X,20,-1,5,WAIT_STAGE|MAINTAIN_THRUST|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|TVR=5:0.5:8|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,12,400,ACCEL_RIGHT_UPPER_LEG,Z,2.5,-0.5,0.5,ACCEL_RIGHT_LOWER_LEG,Z,0,-7,5,ACCEL_RIGHT_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|IF_NOT_A1_MAXTHRUST=9|IF_NOT_A2_MAXTHRUST=5|MAINTAIN_THRUST|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|ABORT_ANGLE0=20)
#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.5:4:3|MAX_THRUST=4|MAX_REVERSE_THRUST=2|MAX_ACCEL=5|MIN_VELOCITY=5|MAX_VELOCITY=10|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,14,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_RIGHT_LEG,14,1000,ACCEL_HIP,X,0,-1,1,WAIT_STAGE|MAX_THRUST=14|MAX_REVERSE_THRUST=3|TVR=0.3:4:4)
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:4:3|MAX_THRUST=4|MAX_REVERSE_THRUST=2|MAX_ACCEL=5|MIN_VELOCITY=5|MAX_VELOCITY=10|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:4:3|MAX_THRUST=4|MAX_REVERSE_THRUST=2|MAX_ACCEL=5|MIN_VELOCITY=5|MAX_VELOCITY=10|FUTURE_LOG)
#possibly right leg motor needs to hold up leg or possibly falls down

12000,StopLogging

