#RightStepUpDownQuick01b.sh
#for 24rpm ankle motors
0,StartLogging 
0,LogScriptFileName
#StartTimeInMS,HoldAngleWithMotor(AccelName,AccelAngleXYorZ,TargetAngle,DegreesPlus,DegreesMinus,MotorName,Dir*Strength,Duration/TimeOut,flags)

#***legs spread 12cm from left foot to right foot center*** - otherwise stage 2 does not work

#constant thrust step right
#no thrust analysis, but does check for angle

0,StoreAngle(ACCEL_LEFT_LOWER_LEG,X,LLLX0)  #-11.8
0,StoreAngle(ACCEL_RIGHT_LOWER_LEG,X,RLLX0)
0,StoreAngle(ACCEL_LEFT_LOWER_LEG,Z,LLLZ0)
0,StoreAngle(ACCEL_RIGHT_LOWER_LEG,Z,RLLZ0)
0,StoreAngle(ACCEL_TORSO,Z,TZ0)
0,StoreAngle(ACCEL_TORSO,X,TX0)
0,StoreAngle(ACCEL_HIP,X,HX0)
0,StoreAngle(ACCEL_RIGHT_UPPER_LEG,X,RULX0)
0,StoreAngle(ACCEL_RIGHT_UPPER_LEG,Z,RULZ0)
0,StoreAngle(ACCEL_LEFT_UPPER_LEG,X,LULX0)
0,StoreAngle(ACCEL_LEFT_UPPER_LEG,Z,LULZ0)

##Stage 1: LEAN LEFT - needs to be somewhat fast, and leaves a little momentum
##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
1,MotorAccel(MOTOR_LEFT_SIDE,3,5000,ACCEL_LEFT_UPPER_LEG,Z,-3,-1,1,WAIT_STAGE|MIN_VELOCITY=5|MAX_VELOCITY=7|MAX_THRUST=3|MAX_REVERSE_THRUST=5|MAX_ACCEL=0.1|NO_STOP_ON_TARGET|MAINTAIN_THRUST)
1,MotorAccel(MOTOR_RIGHT_SIDE,3,5000,ACCEL_LEFT_UPPER_LEG,Z,-3,-1,1,WAIT_STAGE|HOLD_ANGLE|MIN_VELOCITY=5|MAX_VELOCITY=7|MAX_THRUST=3|MAX_REVERSE_THRUST=5|MAX_ACCEL=0.1|NO_STOP_ON_TARGET|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_UPPER_LEG,Z,-3,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=5|MAX_REVERSE_THRUST=3|MIN_VELOCITY=5|MAX_VELOCITY=7|MAX_ACCEL=0.1|NO_STOP_ON_TARGET|FUTURE_LOG|MAINTAIN_THRUST)
1,MotorAccel(MOTOR_RIGHT_ANKLE,3,5000,ACCEL_RIGHT_FOOT,Z,0,-10,5,ACCEL_LEFT_UPPER_LEG,Z,-3,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=5|MAX_REVERSE_THRUST=3|MIN_VELOCITY=5|MAX_VELOCITY=7|MAX_ACCEL=0.1|NO_STOP_ON_TARGET|MAINTAIN_THRUST)
1,MotorAccel(MOTOR_TORSO,1,5000,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=2|MAX_REVERSE_THRUST=2|MIN_ACCEL=0.1|MAINTAIN_THRUST|MAX_VELOCITY=5|OPPOSE_MOTION=0.25:0.75)



#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_UPPER_LEG,Z,-4,-1,1,ACCEL_TORSO,Z,-7,-2,1,WAIT_STAGE|MAX_THRUST0=4|MAX_REVERSE_THRUST0=4|MIN_VELOCITY0=4|MAX_VELOCITY0=7|MAX_THRUST1=6|MAX_REVERSE_THRUST1=4|MIN_VELOCITY1=20|MAX_VELOCITY1=40|MAX_ACCEL0=1|MAX_ACCEL1=3|FUTURE_LOG|ABORT_ON_TIMEOUT)
2,MotorAccel(MOTOR_RIGHT_SIDE,4,4000,ACCEL_LEFT_UPPER_LEG,Z,-4,-1,1,ACCEL_TORSO,Z,-7,-2,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST0=4|MAX_REVERSE_THRUST0=4|MIN_VELOCITY0=4|MAX_VELOCITY0=7|MAX_THRUST1=6|MAX_REVERSE_THRUST1=4|MIN_VELOCITY1=20|MAX_VELOCITY1=40|MAX_ACCEL0=1|MAX_ACCEL1=3)
#|NO_STOP_ON_TARGET
#***NO HOLD_ANGLE will force LEFT_ANKLE angle to be in range (change from -6 to -5)***
#if V=N (V>0), in beginning, bu V is <1, use V*3 to make sure thrust is high enough to lean robot
#ankle angle needs to be more than stage 1, so that ankle does not brake too heavily
#important to not have ankle motor thrusting to brake or in right direction when stage 3 starts
2,MotorAccel(MOTOR_LEFT_ANKLE,4,4000,ACCEL_LEFT_FOOT,Z,0,-20,10,ACCEL_LEFT_UPPER_LEG,Z,-4,-1,1,WAIT_STAGE|MAX_THRUST=7|MAX_REVERSE_THRUST=5|MIN_VELOCITY=7|MAX_VELOCITY=10|ABORT_ANGLE0=30|HOLD_ANGLE=20:5|MAX_ACCEL1=0.1|MAINTAIN_THRUST|FUTURE_LOG)
#2,MotorAccel(MOTOR_LEFT_ANKLE,4,4000,ACCEL_LEFT_FOOT,Z,0,-20,10,ACCEL_LEFT_UPPER_LEG,Z,-4,-1,1,WAIT_STAGE|CONSTANT_THRUST=3:3|ABORT_ANGLE0=30|HOLD_ANGLE=20:5|MAINTAIN_THRUST|NO_STOP_ON_TARGET|FUTURE_LOG)
2,MotorAccel(MOTOR_RIGHT_ANKLE,4,4000,ACCEL_RIGHT_FOOT,Z,0,-10,5,ACCEL_LEFT_UPPER_LEG,Z,-4,-1,1,WAIT_STAGE|HOLD_ANGLE=20:5|MAX_THRUST=7|MAX_REVERSE_THRUST=5|MIN_VELOCITY=7|MAX_VELOCITY=10|MAX_ACCEL1=0.1)
#2,MotorAccel(MOTOR_RIGHT_ANKLE,4,4000,ACCEL_RIGHT_FOOT,Z,0,-10,5,ACCEL_LEFT_UPPER_LEG,Z,-4,-1,1,WAIT_STAGE|CONSTANT_THRUST=3:3|HOLD_ANGLE=20:5|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|MAX_THRUST=2|MAX_REVERSE_THRUST=2|MAINTAIN_THRUST|MAX_VELOCITY=10|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 16/18 slower is 12/14
3,MotorAccel(MOTOR_RIGHT_LEG,18,1000,ACCEL_RIGHT_UPPER_LEG,X,18,-1,5,WAIT_STAGE|CONSTANT_THRUST)
3,MotorAccel(MOTOR_LEFT_LEG,18,1000,ACCEL_RIGHT_UPPER_LEG,X,18,-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,17,1000,ACCEL_RIGHT_LOWER_LEG,X,-20,-1,1,WAIT_STAGE|CONSTANT_THRUST|HOLD_ANGLE)
3,MotorAccel(MOTOR_LEFT_KNEE,3,1000,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 ***
#constant thrust will set t=0 if in range, and thrust should be constant here, so make range very small.
3,MotorAccel(MOTOR_LEFT_ANKLE,12,1000,ACCEL_LEFT_FOOT,Z,0,-10,10,ACCEL_LEFT_UPPER_LEG,Z,-3.5,0,0,WAIT_STAGE|HOLD_ANGLE=10.0:2.0|CONSTANT_THRUST=10:20|MAINTAIN_THRUST|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,1000,ACCEL_RIGHT_FOOT,Z,0,-10,5,ACCEL_LEFT_UPPER_LEG,Z,-3.5,-2,2,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=15|MAX_REVERSE_THRUST=2|MAX_VELOCITY=5)
#3,MotorAccel(MOTOR_LEFT_SIDE,20,1000,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
#3,MotorAccel(MOTOR_LEFT_SIDE,12,1000,ACCEL_LEFT_UPPER_LEG,Z,-2,-3,8,ACCEL_TORSO,Z,-7,-1,1,WAIT_STAGE|HOLD_ANGLE=40:0|MAX_THRUST=18|MAX_REVERSE_THRUST=2|MIN_VELOCITY=50|MAX_VELOCITY=100|MAX_ACCEL=10.0|MAINTAIN_THRUST|NO_STOP_ON_TARGET|FUTURE_LOG)
3,MotorAccel(MOTOR_LEFT_SIDE,12,1000,ACCEL_TORSO,Z,-7,-1,1,WAIT_STAGE|HOLD_ANGLE=40:0|CONSTANT_THRUST=2:20|MAINTAIN_THRUST|NO_STOP_ON_TARGET|FUTURE_LOG)
#was -12, now 24rpm allow is to be more accurate
#make MIN_VELOCITY and MAX_ACCEL very high so that intial thrust will remain all the way to target
3,MotorAccel(MOTOR_RIGHT_SIDE,16,1000,ACCEL_RIGHT_UPPER_LEG,Z,-8,-1,1,WAIT_STAGE|HOLD_ANGLE=100:0|MAX_THRUST=16|MAX_REVERSE_THRUST=2|MIN_VELOCITY=50|MAX_VELOCITY=100|MAX_ACCEL=20.0|NO_STOP_ON_TARGET|MAINTAIN_THRUST|FUTURE_LOG)
#HOLD_ANGLE is needed to nt hold up the end of the stage
#3,MotorAccel(MOTOR_RIGHT_SIDE,20,1000,ACCEL_RIGHT_UPPER_LEG,Z,-10,-1,1,WAIT_STAGE|HOLD_ANGLE|CONSTANT_THRUST=0:20|MAINTAIN_THRUST|FUTURE_LOG)
#3,MotorAccel(MOTOR_RIGHT_SIDE,20,1000,ACCEL_RIGHT_UPPER_LEG,Z,-10,-1,1,WAIT_STAGE|HOLD_ANGLE|OPPOSE_MOTION_MAINTAIN=20:0|MAX_THRUST=18|MAX_REVERSE_THRUST=2|MIN_VELOCITY=20|MAX_VELOCITY=40|MAX_ACCEL=5.0|MAINTAIN_THRUST)
#send LLL forward slightly to stop fall backwards -12 to -14
3,MotorAccel(MOTOR_LEFT_FOOT,5,1000,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,1000,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)
3,MotorAccel(MOTOR_TORSO,3,10000,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=10|MAX_REVERSE_THRUST=3|MAX_VELOCITY=10|MAX_ACCEL=5|MAINTAIN_THRUST)
#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,1000,ACCEL_RIGHT_FOOT,X,0,-5,5,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=12|MAX_REVERSE_THRUST=3|MAX_VELOCITY=5)


#Stage 4: LOWER RIGHT LEG SWINGS BACK, UPPER LEG COMES BACK DOWN, left side holds right leg off the ground 
#***note no HOLD_ANGLE***
4,MotorAccel(MOTOR_RIGHT_LEG,15,700,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-1,1,WAIT_STAGE|CONSTANT_THRUST)
4,MotorAccel(MOTOR_LEFT_LEG,15,700,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-1,1,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
#4,MotorAccel(MOTOR_RIGHT_KNEE,17,700,ACCEL_RIGHT_LOWER_LEG,X,RLLX0,-1,1,WAIT_STAGE|CONSTANT_THRUST|HOLD_ANGLE|ABORT_ANGLE0=-30)
#not constant rt knee
4,MotorAccel(MOTOR_RIGHT_KNEE,17,700,ACCEL_RIGHT_LOWER_LEG,X,RLLX0,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=17|MAX_REVERSE_THRUST=10|MAX_VELOCITY=40|ABORT_ANGLE0=-30)
#if left foot needs to be put back, make sure minv, maxv, and maxa are high enough to make it fast
4,MotorAccel(MOTOR_LEFT_ANKLE,3,700,ACCEL_LEFT_FOOT,Z,0,-10,10,ACCEL_LEFT_UPPER_LEG,Z,-2,-1,1,WAIT_STAGE|HOLD_ANGLE=20:2|MAX_THRUST=20|MAX_REVERSE_THRUST=2|ABORT_ANGLE0=30|MIN_VELOCITY0=20|MAX_VELOCITY0=40|MAX_ACCEL0=5|MAINTAIN_THRUST)
#strive for -7 because otherwise left side collapses, and right leg falls inward too
#4,MotorAccel(MOTOR_LEFT_SIDE,6,800,ACCEL_TORSO,Z,-8,-2,1,ACCEL_RIGHT_UPPER_LEG,Z,-13,-1,1,WAIT_STAGE|HOLD_ANGLE=10:0|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MAINTAIN_THRUST|FUTURE_LOG)
#4,MotorAccel(MOTOR_LEFT_SIDE,6,700,ACCEL_RIGHT_UPPER_LEG,Z,-6,-2,2,ACCEL_TORSO,Z,-8,-2,1,WAIT_STAGE|HOLD_ANGLE=10:0|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MAINTAIN_THRUST|FUTURE_LOG)
#9rpm was MAX_THRUST=20
#the LSM should never turn - to help the LLLZ un it is > 3 or 6, only help to keep it from -6 to -8
#if LULZ > 8 or 9 LSM needs to reverse and turn -
4,MotorAccel(MOTOR_LEFT_SIDE,6,700,ACCEL_LEFT_UPPER_LEG,Z,-2,-2,10,ACCEL_TORSO,Z,-8,-2,1,WAIT_STAGE|HOLD_ANGLE=10:0|MAX_THRUST=16|MAX_REVERSE_THRUST=2|MAINTAIN_THRUST|FUTURE_LOG)
#was -12
#4,MotorAccel(MOTOR_RIGHT_SIDE,10,700,ACCEL_RIGHT_UPPER_LEG,Z,-6,-1,1,WAIT_STAGE|HOLD_ANGLE=10:0|MAX_THRUST=10|MAX_REVERSE_THRUST=2)
4,MotorAccel(MOTOR_RIGHT_SIDE,10,700,ACCEL_RIGHT_UPPER_LEG,Z,-6,-1,1,WAIT_STAGE|CONSTANT_THRUST=0:18)
4,MotorAccel(MOTOR_TORSO,3,800,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=10|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20|MAX_ACCEL=5|MAINTAIN_THRUST)
#use 2:2 because can fall backwards or forwards and needs more thrust
4,MotorAccel(MOTOR_LEFT_FOOT,10,700,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,700,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)
#right ankle should prepare to land and help RUL 
4,MotorAccel(MOTOR_RIGHT_ANKLE,10,700,ACCEL_RIGHT_FOOT,Z,0,-2,2,ACCEL_RIGHT_UPPER_LEG,Z,RULZ0,-2,2,WAIT_STAGE|HOLD_ANGLE=1:1|MAX_THRUST=12|MAX_REVERSE_THRUST=6)
#right foot motors should straighten right foot
4,MotorAccel(MOTOR_RIGHT_FOOT,10,700,ACCEL_RIGHT_FOOT,X,0,-2,2,ACCEL_RIGHT_LOWER_LEG,X,RLLX0,-2,2,WAIT_STAGE|HOLD_ANGLE=1:1|MAX_THRUST=12|MAX_REVERSE_THRUST=6)
#end stage if Right foot front or back touch sensors > 10
4,TouchSensor(TOUCH_RIGHT_FOOT_FRONT,GT,6,700)
4,TouchSensor(TOUCH_RIGHT_FOOT_BACK,GT,6,700)

#Stage 5: BALANCE stand for 3 seconds
#(Basically try to lean back to the center and hold those angles)
#5,MotorAccel(MOTOR_LEFT_SIDE,3,3000,ACCEL_LEFT_UPPER_LEG,Z,LULZ0,-1,1,ACCEL_TORSO,Z,0,-1,1,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=7|MAX_REVERSE_THRUST=3)
5,MotorAccel(MOTOR_LEFT_SIDE,3,3000,ACCEL_HIP,Z,0,-1,1,ACCEL_LEFT_UPPER_LEG,Z,LULZ0,-1,1,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=7|MAX_REVERSE_THRUST=3|MAX_ACCEL=10|MIN_VELOCITY=20|MAX_VELOCITY=50)
#5,MotorAccel(MOTOR_RIGHT_SIDE,3,3000,ACCEL_RIGHT_UPPER_LEG,Z,RULZ0,-1,1,ACCEL_TORSO,Z,0,-1,1,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=7|MAX_REVERSE_THRUST=3)
5,MotorAccel(MOTOR_RIGHT_SIDE,3,3000,ACCEL_HIP,Z,0,-1,1,ACCEL_RIGHT_UPPER_LEG,Z,RULZ0,-1,1,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=7|MAX_REVERSE_THRUST=3|MAX_ACCEL=10|MIN_VELOCITY=20|MAX_VELOCITY=50|)
#the legs need to push back the weight of the torso in the +X
5,MotorAccel(MOTOR_LEFT_LEG,3,3000,ACCEL_HIP,X,0,-1,1,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=6|MAX_REVERSE_THRUST=3|MIN_VELOCITY=20|MAX_VELOCITY=50|MAX_ACCEL=1)
5,MotorAccel(MOTOR_RIGHT_LEG,3,3000,ACCEL_HIP,X,0,-1,1,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=6|MAX_REVERSE_THRUST=3|MIN_VELOCITY=20|MAX_VELOCITY=50|MAX_ACCEL=1)
#5,MotorAccel(MOTOR_RIGHT_KNEE,16,3000,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-1,1,ACCEL_HIP,Z,0,-1,1,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST0=8|MAX_REVERSE_THRUST0=8|MAX_THRUST1=3|MAX_REVERSE_THRUST1=3|MAX_VELOCITY=20|ABORT_ANGLE0=40)  
#5,MotorAccel(MOTOR_LEFT_KNEE,16,3000,ACCEL_LEFT_UPPER_LEG,X,LULX0,-1,1,ACCEL_HIP,Z,0,-1,1,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST0=8|MAX_REVERSE_THRUST0=8|MAX_THRUST1=3|MAX_REVERSE_THRUST1=3|MAX_VELOCITY0=20|MAX_VELOCITY1=10)
5,MotorAccel(MOTOR_RIGHT_KNEE,8,3000,ACCEL_HIP,Z,0,-1,1,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-1,1,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST0=7|MAX_REVERSE_THRUST0=7|MAX_THRUST1=6|MAX_REVERSE_THRUST1=6|MAX_VELOCITY=20|MAX_ACCEL=1.0|ABORT_ANGLE0=40)  
5,MotorAccel(MOTOR_LEFT_KNEE,8,3000,ACCEL_HIP,Z,0,-1,1,ACCEL_LEFT_UPPER_LEG,X,LULX0,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST0=7|MAX_REVERSE_THRUST0=7|MAX_THRUST1=6|MAX_REVERSE_THRUST1=6|MAX_VELOCITY0=20|MAX_VELOCITY1=10|MAX_ACCEL=1.0)
5,MotorAccel(MOTOR_LEFT_ANKLE,3,3000,ACCEL_LEFT_FOOT,Z,0,-10,10,ACCEL_LEFT_UPPER_LEG,Z,LULZ0,-3,3,WAIT_STAGE|HOLD_ANGLE=0.5:1|MAX_THRUST=6|MAX_REVERSE_THRUST=3|MAX_ACCEL=0.1|MAX_VELOCITY=20|MIN_VELOCITY=5|FUTURE_LOG|ABORT_ANGLE0=30)
5,MotorAccel(MOTOR_RIGHT_ANKLE,3,3000,ACCEL_RIGHT_FOOT,Z,0,-10,10,ACCEL_RIGHT_UPPER_LEG,Z,RULZ0,-3,3,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=6|MAX_REVERSE_THRUST=3|MAX_ACCEL=0.1|ABORT_ANGLE0=30)
5,MotorAccel(MOTOR_LEFT_FOOT,10,3000,ACCEL_LEFT_FOOT,X,0,-10,10,ACCEL_LEFT_LOWER_LEG,X,LLLX0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.5:2|MAX_THRUST=16|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20|ABORT_ANGLE0=20)
5,MotorAccel(MOTOR_RIGHT_FOOT,10,3000,ACCEL_RIGHT_FOOT,X,0,-10,10,ACCEL_RIGHT_LOWER_LEG,X,RLLX0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.5:2|MAX_THRUST=16|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20|ABORT_ANGLE0=20)
#torso oppose motion
#maxthrust was 10- needs something better to avoid falling over backwards if angle >-5
5,MotorAccel(MOTOR_TORSO,3,3000,ACCEL_TORSO,X,TX0,-1,1,WAIT_STAGE|HOLD_ANGLE=0.5:1|MAX_THRUST=8|MAX_REVERSE_THRUST=2|MIN_VELOCITY=10|MAX_VELOCITY=40|MAX_ACCEL=5|FUTURE_LOG)

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

10000,StopLogging

