#RightStepUpDown05.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_LOWER_LEG,Z,-7,-1,1,WAIT_STAGE|MIN_VELOCITY=5|MAX_VELOCITY=7|MAX_THRUST=3|MAX_REVERSE_THRUST=5|MAX_ACCEL=0.1|MAINTAIN_THRUST|ABORT_ON_TIMEOUT)
1,MotorAccel(MOTOR_RIGHT_SIDE,3,5000,ACCEL_LEFT_LOWER_LEG,Z,-7,-1,1,WAIT_STAGE|HOLD_ANGLE|MIN_VELOCITY=5|MAX_VELOCITY=7|MAX_THRUST=3|MAX_REVERSE_THRUST=5|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,-7,-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,-7,-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_TORSO,1,5000,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE=1:1|MAX_THRUST=4|MAX_REVERSE_THRUST=2|MAINTAIN_THRUST|MAX_VELOCITY=10|MIN_ACCEL=1)
#NO_STOP_ON_TARGET|


#stage 2: LEFT_SIDE reverses, to make torso Z straighten, and put torso weight over left leg, can be more positive than 1deg
#2,MotorAccel(MOTOR_LEFT_SIDE,4,4000,ACCEL_LEFT_LOWER_LEG,Z,-7,-2,0.5,ACCEL_TORSO,Z,-5,-1,1,WAIT_STAGE|MAX_THRUST0=4|MAX_REVERSE_THRUST0=4|MIN_VELOCITY0=5|MAX_VELOCITY0=8|MAX_THRUST1=10|MAX_REVERSE_THRUST1=4|MIN_VELOCITY1=20|MAX_VELOCITY1=40|MAX_ACCEL0=0.1|MAX_ACCEL1=5|FUTURE_LOG|NO_STOP_ON_TARGET|ABORT_ON_TIMEOUT)
#sometimes the LULZ is apparently reaching even nearly -8, so make sure stage can end if that happens
2,MotorAccel(MOTOR_LEFT_SIDE,4,4000,ACCEL_LEFT_UPPER_LEG,Z,-2,-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=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)
#important note: right side motor picks up momentum in stage 2, but needs to reverse for right leg lift (stage 3), 
2,MotorAccel(MOTOR_LEFT_ANKLE,4,4000,ACCEL_LEFT_LOWER_LEG,Z,-7.0,-1,0.5,ACCEL_LEFT_UPPER_LEG,Z,-2,-3,1,WAIT_STAGE|MAX_THRUST=6|MAX_REVERSE_THRUST=6|MIN_VELOCITY=5|MAX_VELOCITY=8|ABORT_ANGLE0=30|HOLD_ANGLE=20:5|MAX_ACCEL1=0.1|NO_STOP_ON_TARGET|MAINTAIN_THRUST|FUTURE_LOG)
#2,MotorAccel(MOTOR_RIGHT_ANKLE,4,4000,ACCEL_RIGHT_FOOT,Z,0,-10,5,ACCEL_LEFT_LOWER_LEG,Z,-7.0,-1,0.5,WAIT_STAGE|HOLD_ANGLE=20:5|MAX_THRUST=6|MAX_REVERSE_THRUST=6|MIN_VELOCITY=5|MAX_VELOCITY=8|MAX_ACCEL1=0.1|NO_STOP_ON_TARGET)
2,MotorAccel(MOTOR_RIGHT_ANKLE,4,4000,ACCEL_LEFT_LOWER_LEG,Z,-7.0,-1,0.5,ACCEL_LEFT_UPPER_LEG,Z,-2,-3,1,WAIT_STAGE|HOLD_ANGLE=20:5|MAX_THRUST=6|MAX_REVERSE_THRUST=6|MIN_VELOCITY=5|MAX_VELOCITY=8|MAX_ACCEL1=0.1|NO_STOP_ON_TARGET)
#|NO_STOP_ON_TARGET
#feet hold up lower legs
2,MotorAccel(MOTOR_LEFT_FOOT,3,4000,ACCEL_LEFT_LOWER_LEG,X,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE=0.25:0.5|MAX_THRUST=7|MAX_REVERSE_THRUST=3|MAX_VELOCITY=10)
2,MotorAccel(MOTOR_RIGHT_FOOT,3,4000,ACCEL_RIGHT_LOWER_LEG,X,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE=0.25:0.5|MAX_THRUST=7|MAX_REVERSE_THRUST=3|MAX_VELOCITY=10)
2,MotorAccel(MOTOR_TORSO,3,4000,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE=1:1|MAX_THRUST=4|MAX_REVERSE_THRUST=2|MAINTAIN_THRUST|MAX_VELOCITY=10|MIN_ACCEL=1)
#OPPOSE_MOTION=0.25:0.75

#Stage 3: LIFT RIGHT LEG only (no knee or foot movement) 
#(LEFT FOOT MOTOR HOLDS ROBOT FROM FALLING FORWARD)
#todo: allow MAINTAIN_THRUST flag to work on MOTOR_RIGHT_LEG  +3 because can skip from 19 to 21
#was angle=16
#important: changing these 3 20s to 18s can cause the EthMotors PCB (presumably PIC) to stop communication, presumably from EMF, and noise can be seen in the Touch sensor lines.
3,MotorAccel(MOTOR_RIGHT_LEG,20,600,ACCEL_RIGHT_UPPER_LEG,X,15,-1,5,WAIT_STAGE|CONSTANT_THRUST)
3,MotorAccel(MOTOR_LEFT_LEG,20,600,ACCEL_RIGHT_UPPER_LEG,X,15,-1,5,WAIT_STAGE|CONSTANT_THRUST|HOLD_ANGLE)
#bend right knee so foot lifts in place - decrease angle X (lower leg rotates forward)
#14=too much momentum into stage 4
3,MotorAccel(MOTOR_RIGHT_KNEE,20,600,ACCEL_RIGHT_LOWER_LEG,X,-20,-1,1,WAIT_STAGE|CONSTANT_THRUST|HOLD_ANGLE)
3,MotorAccel(MOTOR_LEFT_KNEE,3,600,ACCEL_LEFT_UPPER_LEG,X,LULX0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=9|MAX_REVERSE_THRUST=6|MAINTAIN_THRUST|ABORT_ANGLE0=25)
#TODO: *** MAINTAIN_THRUST is not working here ***
#3,MotorAccel(MOTOR_LEFT_ANKLE,12,600,ACCEL_LEFT_LOWER_LEG,Z,-7,-1,0,WAIT_STAGE|HOLD_ANGLE=20.0:2.0|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MAX_VELOCITY=5|MAINTAIN_THRUST|ABORT_ANGLE0=30|FUTURE_LOG)
#MAX_THRUST for 9rpm was=20
3,MotorAccel(MOTOR_LEFT_ANKLE,12,600,ACCEL_LEFT_FOOT,Z,0,-10,10,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|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,600,ACCEL_RIGHT_FOOT,Z,0,-10,5,ACCEL_LEFT_LOWER_LEG,Z,-8,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MAX_VELOCITY=5|MAX_ACCEL=0.5)
#3,MotorAccel(MOTOR_LEFT_SIDE,20,600,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,
3,MotorAccel(MOTOR_LEFT_SIDE,12,600,ACCEL_LEFT_LOWER_LEG,Z,-6,-1,6,ACCEL_TORSO,Z,-8,-1,1,WAIT_STAGE|HOLD_ANGLE=40:0|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MIN_VELOCITY=20|MAX_VELOCITY=40|MAX_ACCEL=5.0|MAINTAIN_THRUST|NO_STOP_ON_TARGET|FUTURE_LOG)
#3,MotorAccel(MOTOR_LEFT_SIDE,12,600,ACCEL_LEFT_LOWER_LEG,Z,-6,-2,6,ACCEL_TORSO,Z,-8,-1,1,WAIT_STAGE|HOLD_ANGLE=40:0|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MIN_VELOCITY=20|MAX_VELOCITY=40|MAX_ACCEL=5.0|MAINTAIN_THRUST|NO_STOP_ON_TARGET|CONSTANT_THRUST=2:7|FUTURE_LOG)
#was -12, now 24rpm allow is to be more accurate
#3,MotorAccel(MOTOR_RIGHT_SIDE,20,600,ACCEL_RIGHT_UPPER_LEG,Z,-10,-1,1,WAIT_STAGE|HOLD_ANGLE=20:0|MAX_THRUST=10|MAX_REVERSE_THRUST=2|MIN_VELOCITY=5|MAX_VELOCITY=40|MAX_ACCEL=1.0|MAINTAIN_THRUST)
#RSM strives for LULZ-6 (actual angle is LULZ-6, but use -10 in stage 3 to make sure thrust is <0 using -10 possibly was why the left side motor case broke)
#3,MotorAccel(MOTOR_RIGHT_SIDE,10,600,ACCEL_RIGHT_UPPER_LEG,Z,-6,-1,1,ACCEL_LEFT_UPPER_LEG,Z,0,0,0,WAIT_STAGE|HOLD_ANGLE=20:0|MAX_THRUST=10|MAX_REVERSE_THRUST=2|MIN_VELOCITY=5|MAX_VELOCITY=40|MAX_ACCEL=3.0|RELATIVE_ANGLE|NO_STOP_ON_TARGET|MAINTAIN_THRUST)
#3,MotorAccel(MOTOR_RIGHT_SIDE,20,600,ACCEL_RIGHT_UPPER_LEG,Z,-10,-1,1,WAIT_STAGE|HOLD_ANGLE=20:0|MAX_THRUST=18|MAX_REVERSE_THRUST=2|MIN_VELOCITY=5|MAX_VELOCITY=40|MAX_ACCEL=1.0|CONSTANT_THRUST=0:7|MAINTAIN_THRUST)
#CurrentAnglePlus -3 worked ok but went too far once
#3,MotorAccel(MOTOR_RIGHT_SIDE,10,600,ACCEL_RIGHT_UPPER_LEG,Z,CurrentAnglePlus,-2,-1,1,WAIT_STAGE|HOLD_ANGLE=20:0|MAX_THRUST=12|MAX_REVERSE_THRUST=2|MIN_VELOCITY=5|MAX_VELOCITY=40|MAX_ACCEL=3.0|NO_STOP_ON_TARGET|MAINTAIN_THRUST)
3,MotorAccel(MOTOR_RIGHT_SIDE,13,600,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_ANGLE|MAINTAIN_THRUST)
#send LLL forward slightly to stop fall backwards -12 to -14
3,MotorAccel(MOTOR_LEFT_FOOT,5,600,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,600,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|MIN_VELOCITY=0|MAX_VELOCITY=10|MAX_ACCEL=0.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,600,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***
#end stage if Right foot front or back touch sensors > 3
#required to end stage 4: RULX reaches RULX0 +-1 or Right foot front or back Touch Sensor > 3
4,TouchSensor(TOUCH_RIGHT_FOOT_FRONT,GT,3.0,250,400)
4,TouchSensor(TOUCH_RIGHT_FOOT_BACK,GT,3.0,250,400)
4,MotorAccel(MOTOR_RIGHT_LEG,20,400,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-1,1,WAIT_STAGE|CONSTANT_THRUST)
4,MotorAccel(MOTOR_LEFT_LEG,20,400,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
#target angle was RLLX0 (-12), but needs to be relative angle (RULX-18  so when RULX=21 LULX target will be +3 and work down to -12
#4,MotorAccel(MOTOR_RIGHT_KNEE,17,700,ACCEL_RIGHT_LOWER_LEG,X,3,0,0,WAIT_STAGE|CONSTANT_THRUST=20:5|HOLD_ANGLE|ABORT_ANGLE0=-30)
4,MotorAccel(MOTOR_RIGHT_KNEE,20,700,ACCEL_RIGHT_LOWER_LEG,X,-18,-1,1,ACCEL_RIGHT_UPPER_LEG,X,0,0,0,WAIT_STAGE|HOLD_ANGLE|RELATIVE_ANGLE|ABORT_ANGLE0=-40|MAX_ACCEL=10|MIN_VELOCITY=30|MAX_VELOCITY=50|MAX_THRUST=18|CONSTANT_THRUST18:4)
#4,MotorAccel(MOTOR_RIGHT_KNEE,18,700,ACCEL_RIGHT_LOWER_LEG,X,RLLX0,-1,1,WAIT_STAGE|HOLD_ANGLE|ABORT_ANGLE0=-30|MAX_ACCEL=10|MIN_VELOCITY=30|MAX_VELOCITY=50|MAX_THRUST=18|CONSTANT_THRUST18:4)
#not constant rt knee
#4,MotorAccel(MOTOR_RIGHT_KNEE,20,400,ACCEL_RIGHT_LOWER_LEG,X,RLLX0,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=20|MAX_REVERSE_THRUST=10|MAX_VELOCITY=40|ABORT_ANGLE0=-30)
4,MotorAccel(MOTOR_LEFT_ANKLE,3,400,ACCEL_LEFT_FOOT,Z,0,-10,10,ACCEL_LEFT_LOWER_LEG,Z,-6,-1,0,WAIT_STAGE|HOLD_ANGLE=20:2|MAX_THRUST=20|MAX_REVERSE_THRUST=2|ABORT_ANGLE0=30|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
#LSM can cause a larger than wanted side step if thrust is too high
#4,MotorAccel(MOTOR_LEFT_SIDE,6,400,ACCEL_LEFT_LOWER_LEG,Z,-6,-2,12,ACCEL_TORSO,Z,-8,-2,1,WAIT_STAGE|HOLD_ANGLE=10:0|MAX_THRUST=16|MAX_REVERSE_THRUST=2|MAINTAIN_THRUST|FUTURE_LOG)
#try RELATIVE_ANGLE, +6 (LULZ0=3 and RULZ0=-3)
4,MotorAccel(MOTOR_LEFT_SIDE,6,400,ACCEL_LEFT_UPPER_LEG,Z,8,-1,1,ACCEL_RIGHT_UPPER_LEG,Z,0,0,0,WAIT_STAGE|HOLD_ANGLE=10:0|MAX_THRUST=10|MAX_REVERSE_THRUST=2|MAINTAIN_THRUST|FUTURE_LOG)
#was -12, -3 is RLLZ0, -6 is correct relative angle, but -8 will send a little bit farther
#4,MotorAccel(MOTOR_RIGHT_SIDE,10,400,ACCEL_RIGHT_UPPER_LEG,Z,-6,-1,1,WAIT_STAGE|HOLD_ANGLE=10:1|MAX_THRUST=8|MAX_REVERSE_THRUST=2)
4,MotorAccel(MOTOR_RIGHT_SIDE,13,400,ACCEL_RIGHT_UPPER_LEG,Z,-8,-1,1,ACCEL_LEFT_UPPER_LEG,Z,0,0,0,WAIT_STAGE|HOLD_ANGLE=10:1|MAX_THRUST=13|MAX_REVERSE_THRUST=2|MAX_ACCEL=1|RELATIVE_ANGLE)
4,MotorAccel(MOTOR_TORSO,3,400,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=16|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20|MAX_ACCEL=0.1|MAINTAIN_THRUST)
#|CONSTANT_THRUST=20:4
#use 2:2 because can fall backwards or forwards and needs more thrust
4,MotorAccel(MOTOR_LEFT_FOOT,10,400,ACCEL_LEFT_FOOT,X,0,-10,10,ACCEL_LEFT_LOWER_LEG,X,LLLX0,-1,1,WAIT_STAGE|HOLD_ANGLE=2:2|MAX_THRUST=16|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20)
4,MotorAccel(MOTOR_LEFT_KNEE,10,400,ACCEL_LEFT_UPPER_LEG,X,LULX0,-2,2,ACCEL_HIP,Z,0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=12|MAX_REVERSE_THRUST=6|MAINTAIN_THRUST|ABORT_ANGLE0=30)
#right ankle and right foot motors should straighten right foot
4,MotorAccel(MOTOR_RIGHT_ANKLE,10,400,ACCEL_RIGHT_FOOT,Z,0,-2,2,WAIT_STAGE|HOLD_ANGLE=1:1|MAX_THRUST=18|MAX_REVERSE_THRUST=6)
4,MotorAccel(MOTOR_RIGHT_FOOT,10,400,ACCEL_RIGHT_FOOT,X,0,-2,2,WAIT_STAGE|HOLD_ANGLE=1:1|MAX_THRUST=12|MAX_REVERSE_THRUST=6)


#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_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)
#|USE_ACCEL_WITH_MOST_V - will work on HIP Z if leg Z has less velocity
#NO_MOTION_NO_THRUST0- if upper leg Z is not moving, then probably it is in a stable orientation, so do not thrust any more
5,MotorAccel(MOTOR_LEFT_SIDE,3,3000,ACCEL_LEFT_UPPER_LEG,Z,LULZ0,-3,3,ACCEL_HIP,Z,0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=7|MAX_REVERSE_THRUST=3|USE_ACCEL_WITH_MOST_V|NO_MOTION_NO_THRUST)
5,MotorAccel(MOTOR_RIGHT_SIDE,3,3000,ACCEL_RIGHT_UPPER_LEG,Z,RULZ0,-3,3,ACCEL_HIP,Z,0,-2,2,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST=7|MAX_REVERSE_THRUST=3|USE_ACCEL_WITH_MOST_V|NO_MOTION_NO_THRUST)
#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|MAX_VELOCITY=40|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|MAX_VELOCITY=40|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,-3,3,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-3,3,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST0=5|MAX_REVERSE_THRUST0=5|MAX_THRUST1=6|MAX_REVERSE_THRUST1=6|MAX_VELOCITY0=10|MAX_VELOCITY=10|MAX_ACCEL=0.5|ABORT_ANGLE0=40|USE_ACCEL_WITH_MOST_V|NO_MOTION_NO_THRUST)  
#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=5|MAX_REVERSE_THRUST0=5|MAX_THRUST1=6|MAX_REVERSE_THRUST1=6|MAX_VELOCITY0=20|MAX_VELOCITY1=10|MAX_ACCEL=1.0|FUTURE_LOG)
5,MotorAccel(MOTOR_LEFT_KNEE,8,3000,ACCEL_HIP,Z,0,-3,3,ACCEL_LEFT_UPPER_LEG,X,LULX0,-3,3,WAIT_STAGE|HOLD_ANGLE=0.5:0.5|MAX_THRUST0=5|MAX_REVERSE_THRUST0=5|MAX_THRUST1=6|MAX_REVERSE_THRUST1=6|MAX_VELOCITY0=10|MAX_VELOCITY1=10|MAX_ACCEL=0.5|USE_ACCEL_WITH_MOST_V|NO_MOTION_NO_THRUST|FUTURE_LOG)
#NO_MOTION_NO_THRUST|
5,MotorAccel(MOTOR_LEFT_ANKLE,3,3000,ACCEL_LEFT_FOOT,Z,0,-10,10,ACCEL_LEFT_LOWER_LEG,Z,LLLZ0,-3,3,WAIT_STAGE|HOLD_ANGLE=1:2|MAX_THRUST=9|MAX_REVERSE_THRUST=3|MAX_ACCEL=1|MAX_VELOCITY=30|MIN_VELOCITY=20|NO_MOTION_NO_THRUST1|FUTURE_LOG|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
5,MotorAccel(MOTOR_RIGHT_ANKLE,3,3000,ACCEL_RIGHT_FOOT,Z,0,-10,20,ACCEL_RIGHT_LOWER_LEG,Z,RLLZ0,-3,3,WAIT_STAGE|HOLD_ANGLE=1:2|MAX_THRUST=9|MAX_REVERSE_THRUST=3|MAX_ACCEL=1|MAX_VELOCITY=30|MIN_VELOCITY=20|NO_MOTION_NO_THRUST1|ABORT_ANGLE0=30)
#5,MotorAccel(MOTOR_LEFT_ANKLE,3,3000,ACCEL_LEFT_FOOT,Z,0,-10,10,ACCEL_HIP,Z,0,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=7|MAX_REVERSE_THRUST=3|MAX_ACCEL=0.1|MAX_VELOCITY=10|MIN_VELOCITY=5|FUTURE_LOG|ABORT_ANGLE0=30)
#5,MotorAccel(MOTOR_RIGHT_ANKLE,3,3000,ACCEL_RIGHT_FOOT,Z,0,-10,10,ACCEL_HIP,Z,0,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=7|MAX_REVERSE_THRUST=3|MAX_ACCEL=0.1|MAX_VELOCITY=10|MIN_VELOCITY=5|ABORT_ANGLE0=30)
#NO_MOTION_NO_THRUST1 - if lower leg x is not moving, do not thrust because it probably is stable
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=10|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20|NO_MOTION_NO_THRUST1)
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=10|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20|NO_MOTION_NO_THRUST1)
#5,MotorAccel(MOTOR_LEFT_FOOT,10,3000,ACCEL_LEFT_FOOT,X,0,-5,5,ACCEL_HIP,X,0,-1,1,WAIT_STAGE|HOLD_ANGLE=0.5:2|MAX_THRUST=16|MAX_REVERSE_THRUST=3|MAX_VELOCITY=10|MAX_ACCEL=1)
#5,MotorAccel(MOTOR_RIGHT_FOOT,10,3000,ACCEL_RIGHT_FOOT,X,0,-5,5,ACCEL_HIP,X,0,-1,1,WAIT_STAGE|HOLD_ANGLE=0.5:2|MAX_THRUST=16|MAX_REVERSE_THRUST=3|MAX_VELOCITY=10|MAX_ACCEL=1)
#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=7|MAX_REVERSE_THRUST=2|MIN_VELOCITY=10|MAX_VELOCITY=20|MAX_ACCEL=0.1|FUTURE_LOG)
#there is usually a large - velocity at the beginning of Stage 5
5,MotorAccel(MOTOR_TORSO,3,3000,ACCEL_TORSO,X,TX0,-1,1,WAIT_STAGE|HOLD_ANGLE=1:20|MAX_THRUST=7|MAX_REVERSE_THRUST=2|MIN_VELOCITY=10|MAX_VELOCITY=20|MAX_ACCEL=0.1|NO_MOTION_NO_THRUST|FUTURE_LOG)
#possibly right leg motor needs to hold up leg or possibly falls down

10000,StopLogging

