#LiftRightLeg02.sh
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
##both ankles lean ankle until LLLZ is 0, thrust=7
##alt: left ankle turns until LLLZ=-1 LLLZ starts at 3, important to limit thrust
##possibly 2 motors is the reason stage 1 lasts for a few ms after the target range is reached
##1,MotorAccel(MOTOR_LEFT_SIDE,MOTOR_RIGHT_SIDE,3,5000,ACCEL_LEFT_LOWER_LEG,Z,-4,-2,1,WAIT_STAGE|DOUBLE_THRUST|CONSTANT_THRUST)
1,MotorAccel(MOTOR_LEFT_SIDE,3,5000,ACCEL_LEFT_LOWER_LEG,Z,-7,-1,1,WAIT_STAGE|DOUBLE_THRUST|MAX_VELOCITY=10|MIN_VELOCITY=5|MAX_THRUST=3|MAX_REVERSE_THRUST=3)
1,MotorAccel(MOTOR_RIGHT_SIDE,3,5000,ACCEL_LEFT_LOWER_LEG,Z,-7,-1,1,WAIT_STAGE|HOLD_ANGLE|DOUBLE_THRUST|MAX_VELOCITY=10|MIN_VELOCITY=5|MAX_THRUST=3|MAX_REVERSE_THRUST=3)
1,MotorAccel(MOTOR_LEFT_ANKLE,3,5000,ACCEL_LEFT_FOOT,Z,0,-10,5,ACCEL_LEFT_LOWER_LEG,Z,-7,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=3|MAX_REVERSE_THRUST=3|MAX_VELOCITY=10|MIN_VELOCITY=5|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=3|MAX_REVERSE_THRUST=3|MAX_VELOCITY=10|MIN_VELOCITY=5)
##feet, knee, leg, and torso motors keep torso from falling forward or backward
##torso holds torso x until stage 4- note LAST_STAGE=4 needs 10s timeout, torso should not be using a lot of thrust, only small adjustments
##1,MotorAccel(MOTOR_TORSO,3,10000,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=4|MAX_REVERSE_THRUST=3|LAST_STAGE=4|MAX_VELOCITY=20)
1,MotorAccel(MOTOR_TORSO,3,5000,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=5|MAX_REVERSE_THRUST=3|MAINTAIN_THRUST|MAX_VELOCITY=5)



#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,6,3000,ACCEL_TORSO,Z,1,-1,4,WAIT_STAGE|MAX_THRUST=7)
#inst lasts until stage 4 (balance with foot in air) Roll degrees 1 to -5
#2,MotorAccel(MOTOR_LEFT_SIDE,6,3000,ACCEL_TORSO,Z,-2,-2,1,WAIT_STAGE|MAX_THRUST=9|ABORT_ON_TIMEOUT)
#if LLLZ goes too negative - falls to left, LSM should be +, LLLZ goes positive falls to right, LSM should be -
# high max velocity so that it is not afraid to increase thrust - removed MAINTAIN_THRUST|
2,MotorAccel(MOTOR_LEFT_SIDE,7,4000,ACCEL_LEFT_LOWER_LEG,Z,-7,-2,2,ACCEL_TORSO,Z,-5,-5,3,WAIT_STAGE|MAX_THRUST=12|ABORT_ON_TIMEOUT|MAX_VELOCITY=30|MIN_VELOCITY=10|FUTURE_LOG)
#2,MotorAccel(MOTOR_LEFT_SIDE,5,4000,ACCEL_TORSO,Z,-4,-5,3,WAIT_STAGE|MAX_THRUST=12|ABORT_ON_TIMEOUT|MAX_VELOCITY=30|MIN_VELOCITY=5)
#ABORT_ANGLE0=5 - end script if ankle Z turns past 5 degrees was -6
#  ***Important for the Left Ankle to have a large negative LLLZ range, because the side will make LLLZ positive and if the ankle
# is thrusting in that direction- it builds up too much velocity
2,MotorAccel(MOTOR_LEFT_ANKLE,3,4000,ACCEL_LEFT_FOOT,Z,0,-20,10,ACCEL_LEFT_LOWER_LEG,Z,-8,-2,0,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MAX_VELOCITY=5|ABORT_ANGLE0=30|MAINTAIN_THRUST|OPPOSE_MOTION=7.0:1|FUTURE_LOG)
#legs hold up torso
#2,MotorAccel(MOTOR_LEFT_LEG,MOTOR_RIGHT_LEG,3,5000,ACCEL_TORSO,X,TX0,-1,1,WAIT_STAGE|HOLD_ANGLE|DOUBLE_THRUST|MAX_THRUST=6)
#knee holds up upper legs through last stage
#2,MotorAccel(MOTOR_LEFT_KNEE,3,10000,ACCEL_LEFT_UPPER_LEG,X,CurrentAngle[LULX],-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=9|MAX_REVERSE_THRUST=6|LAST_STAGE=4|ABORT_ANGLE0=25)
2,MotorAccel(MOTOR_LEFT_KNEE,3,4000,ACCEL_LEFT_UPPER_LEG,X,CurrentAngle[LULX],-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=9|MAX_REVERSE_THRUST=6|MAINTAIN_THRUST|ABORT_ANGLE0=25)
2,MotorAccel(MOTOR_RIGHT_KNEE,3,4000,ACCEL_RIGHT_UPPER_LEG,X,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=8)
#feet hold up lower legs
2,MotorAccel(MOTOR_LEFT_FOOT,3,4000,ACCEL_LEFT_LOWER_LEG,X,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE|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|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=4|MAX_REVERSE_THRUST=3|MAINTAIN_THRUST|MAX_VELOCITY=20|OPPOSE_MOTION=0.5:0.25)


#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
3,MotorAccel(MOTOR_RIGHT_LEG,16,600,ACCEL_RIGHT_UPPER_LEG,X,20,-1,3,WAIT_STAGE|CONSTANT_THRUST)
3,MotorAccel(MOTOR_LEFT_LEG,16,600,ACCEL_RIGHT_UPPER_LEG,X,20,-1,1,WAIT_STAGE|CONSTANT_THRUST|HOLD_ANGLE)
#bend right knee so foot lifts in place - decrease angle X (lower leg rotates forward)
3,MotorAccel(MOTOR_RIGHT_KNEE,12,600,ACCEL_RIGHT_LOWER_LEG,X,-20,-1,1,WAIT_STAGE|CONSTANT_THRUST|HOLD_ANGLE)
#left knee bring upper leg from 6 to 10 (was LULX)
3,MotorAccel(MOTOR_LEFT_KNEE,3,600,ACCEL_LEFT_UPPER_LEG,X,10,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=9|MAX_REVERSE_THRUST=6|MAINTAIN_THRUST|ABORT_ANGLE0=25)
#bias left knee for pos
#3,MotorAccel(MOTOR_LEFT_KNEE,3,600,ACCEL_LEFT_UPPER_LEG,X,10,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=9|MAX_REVERSE_THRUST=6|MAINTAIN_THRUST|ABORT_ANGLE0=25|OPPOSE_MOTION=0.25:2.0)
#LKM constant thrust
#3,MotorAccel(MOTOR_LEFT_KNEE,4,600,ACCEL_LEFT_UPPER_LEG,X,20,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=10|MAX_REVERSE_THRUST=6|MAINTAIN_THRUST|ABORT_ANGLE0=25|CONSTANT_THRUST)
#separate inst for left angle in stage 3 because stage 3 (lifting right leg) causes the main pull on the ankle- not stage 2 as much
#and a stronger ankle is needed in stage 3 and 4
#not a big deal for left ankle thrust to =0 at the start of stage 3
#3,MotorAccel(MOTOR_LEFT_ANKLE,3,10000,ACCEL_LEFT_FOOT,Z,0,-10,10,ACCEL_LEFT_LOWER_LEG,Z,-4,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=12|MAX_REVERSE_THRUST=3|ABORT_ANGLE0=15|LAST_STAGE=4)
#3,MotorAccel(MOTOR_LEFT_ANKLE,12,600,ACCEL_LEFT_FOOT,Z,0,-15,15,ACCEL_LEFT_LOWER_LEG,Z,-7,-2,0,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MAX_VELOCITY=5|ABORT_ANGLE0=30|MAINTAIN_THRUST|OPPOSE_MOTION=3.0:0.5|FUTURE_LOG)
3,MotorAccel(MOTOR_LEFT_ANKLE,12,600,ACCEL_LEFT_LOWER_LEG,Z,-7,-2,0,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MAX_VELOCITY=5|MAINTAIN_THRUST|OPPOSE_MOTION=4.0:0.1|FUTURE_LOG)
#important that left knee hold ULX to 17 standing is 10 - left knee needs to push hard initially
#3,MotorAccel(MOTOR_LEFT_SIDE,6,2000,ACCEL_LEFT_LOWER_LEG,Z,-5,-1,1,ACCEL_TORSO,Z,-2,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=9|MAX_REVERSE_THRUST=2|LAST_STAGE=4)
#3,MotorAccel(MOTOR_LEFT_SIDE,6,2000,ACCEL_LEFT_LOWER_LEG,Z,-5,-1,1,ACCEL_TORSO,Z,-2,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=9|MAX_REVERSE_THRUST=2|MAINTAIN_THRUST)
#change range so LSM is going to be constantly thrusting and pulling the RL up (-9 -4 -1) to (-9 -4 -3)
#MaxVelocity=20 because left side needs to trust positively immediately
3,MotorAccel(MOTOR_LEFT_SIDE,10,600,ACCEL_TORSO,Z,-4,-5,0,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MAX_VELOCITY=20|MAINTAIN_THRUST|OPPOSE_MOTION_MAINTAIN=4.0:0.05|FUTURE_LOG)
#3,MotorAccel(MOTOR_LEFT_SIDE,10,600,ACCEL_LEFT_LOWER_LEG,Z,-7,-2,0,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=20|MAX_REVERSE_THRUST=2|MAX_VELOCITY=20|MAINTAIN_THRUST|OPPOSE_MOTION=2.0:0.5|FUTURE_LOG)
#it seems likely that right and left side need to be syncronized with the same thrust magnitude for z stability
#***do not use oppose motion on Right lower leg because it pushes the motor ccw, in the wrong direction***
#3,MotorAccel(MOTOR_LEFT_SIDE,10,600,ACCEL_RIGHT_LOWER_LEG,Z,-12,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=10|MAX_REVERSE_THRUST=2|MAX_VELOCITY=20|MAINTAIN_THRUST|FUTURE_LOG)
#right side pushes away from left leg - right leg tends to fall towards left leg in stage 3
#3,MotorAccel(MOTOR_RIGHT_SIDE,6,2000,ACCEL_RIGHT_LOWER_LEG,Z,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=5|MAX_REVERSE_THRUST=2|LAST_STAGE=4)
#3,MotorAccel(MOTOR_RIGHT_SIDE,10,2000,ACCEL_RIGHT_LOWER_LEG,Z,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=10|MAX_REVERSE_THRUST=2|MAINTAIN_THRUST)
#right side watches RLLZ was -12
#initial thrust is wrongly set to -1 why?- possibly DO_NOT_USE_GRAVITY flag
#OPPOSE_MOTION_MAINTAIN=2.0:0.5
#3,MotorAccel(MOTOR_RIGHT_SIDE,1,600,ACCEL_RIGHT_LOWER_LEG,Z,-12,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=10|MAX_REVERSE_THRUST=2|MAX_VELOCITY=10|MAINTAIN_THRUST|OPPOSE_MOTION=)
3,MotorAccel(MOTOR_RIGHT_SIDE,1,600,ACCEL_RIGHT_UPPER_LEG,Z,-3,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=16|MAX_REVERSE_THRUST=2|MAX_VELOCITY=10|MAINTAIN_THRUST|OPPOSE_MOTION=1.0:0.5)
#left foot brings body forward from -12 to -21 (was CurrentAngle[LLLX])
3,MotorAccel(MOTOR_LEFT_FOOT,5,600,ACCEL_LEFT_FOOT,X,0,-2,2,ACCEL_LEFT_LOWER_LEG,X,-21,-1,1,WAIT_STAGE|HOLD_ANGLE|ABORT_ANGLE1=-30|MAX_THRUST=16|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20)
#right ankle hold Z at 0
3,MotorAccel(MOTOR_RIGHT_ANKLE,10,600,ACCEL_RIGHT_FOOT,Z,0,-3,3,WAIT_STAGE|HOLD_ANGLE)
3,MotorAccel(MOTOR_TORSO,3,10000,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=18|MAX_REVERSE_THRUST=3|MAX_VELOCITY=10|MAINTAIN_THRUST)
#right foot should move RFX to 0 to decrease friction with the floor from the front of the right foot.
3,MotorAccel(MOTOR_RIGHT_FOOT,10,600,ACCEL_RIGHT_FOOT,X,0,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=12|MAX_REVERSE_THRUST=3|MAX_VELOCITY=5)




#Stage 5: BALANCE stand for 3 seconds
#(Basically try to lean back to the center and hold those angles)
#sides
#5,MotorAccel(MOTOR_LEFT_SIDE,MOTOR_RIGHT_SIDE,3,3000,ACCEL_TORSO,Z,0,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=8|MAX_REVERSE_THRUST=3|DOUBLE_THRUST)
#important that side motors work to keep torso Z=0- otherwise robot may fall sideways (was: set both side motors based on lower leg Z to go back to starting Z angle)
#5,MotorAccel(MOTOR_LEFT_SIDE,3,3000,ACCEL_LEFT_LOWER_LEG,Z,LLLZ0,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=8|MAX_REVERSE_THRUST=3|DOUBLE_THRUST)
#5,MotorAccel(MOTOR_RIGHT_SIDE,3,3000,ACCEL_RIGHT_LOWER_LEG,Z,RLLZ0,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=8|MAX_REVERSE_THRUST=3|DOUBLE_THRUST)
#todo: watch upper or lower leg too
#important to have all upper motors focus on torso, since the torso has a lot of weight and is the biggest factor in the robot falling forward/back  or left/right
5,MotorAccel(MOTOR_LEFT_SIDE,3,3000,ACCEL_TORSO,Z,0,-2,2,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=10|MAX_REVERSE_THRUST=3|OPPOSE_MOTION=0.3:0.3)
5,MotorAccel(MOTOR_RIGHT_SIDE,3,3000,ACCEL_TORSO,Z,0,-2,2,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=10|MAX_REVERSE_THRUST=3|OPPOSE_MOTION=0.3:0.3)
#5,MotorAccel(MOTOR_LEFT_SIDE,3,3000,ACCEL_LEFT_LOWER_LEG,Z,LLLZ0,-2,2,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=18|MAX_REVERSE_THRUST=3|OPPOSE_MOTION=1.0:1.0)
#5,MotorAccel(MOTOR_RIGHT_SIDE,3,3000,ACCEL_RIGHT_LOWER_LEG,Z,RLLZ0,-2,2,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=18|MAX_REVERSE_THRUST=3|OPPOSE_MOTION=1.0:1.0)

#legs - both try to hold torso X to starting angle
#was TX0 (-12)  max-thrust=8 = 4 each
#perhaps legs should try to hold both torso and upper/lower leg
#5,MotorAccel(MOTOR_LEFT_LEG,MOTOR_RIGHT_LEG,3,3000,ACCEL_TORSO,X,-15,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=8|MAX_REVERSE_THRUST=3|DOUBLE_THRUST)
5,MotorAccel(MOTOR_LEFT_LEG,3,3000,ACCEL_TORSO,X,-15,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=10|MAX_REVERSE_THRUST=3|OPPOSE_MOTION=0.5:0.8)
5,MotorAccel(MOTOR_RIGHT_LEG,3,3000,ACCEL_TORSO,X,-15,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=10|MAX_REVERSE_THRUST=3|OPPOSE_MOTION=0.5:0.8)
#now RKM just opposes with bias against pos motion, was: RKM needs to push negative upper right leg from around 20 to 15 (was CurrentAngle)
5,MotorAccel(MOTOR_RIGHT_KNEE,9,3000,ACCEL_RIGHT_UPPER_LEG,X,15,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=18|MAX_VELOCITY=20|ABORT_ANGLE0=40|OPPOSE_MOTION=0.5:0.5)
#5,MotorAccel(MOTOR_LEFT_KNEE,9,3000,ACCEL_LEFT_UPPER_LEG,X,CurrentAngle,-1,1,WAIT_STAGE|CONSTANT_THRUST|HOLD_ANGLE)
#LKM can be used to shift robot Z back to left quickly for now hold at 6   (LULX0 is around 6)
#LKM opposes motion heavily biased to fall forward
5,MotorAccel(MOTOR_LEFT_KNEE,16,3000,ACCEL_LEFT_UPPER_LEG,X,6,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=20|MAX_VELOCITY=20|OPPOSE_MOTION=1.0:0.1)
#left ankle needs to go back to the original angle - based on left lower leg Z 3deg
#bias LAM to pos
5,MotorAccel(MOTOR_LEFT_ANKLE,3,3000,ACCEL_LEFT_FOOT,Z,0,-10,10,ACCEL_LEFT_LOWER_LEG,Z,LLLZ0,-1,0,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=20|MAX_REVERSE_THRUST=3|ABORT_ANGLE0=30|OPPOSE_MOTION=0.5:1.0)
#bias RAM to neg
5,MotorAccel(MOTOR_RIGHT_ANKLE,3,3000,ACCEL_RIGHT_FOOT,Z,0,-10,10,ACCEL_RIGHT_LOWER_LEG,Z,RLLZ0,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=20|MAX_REVERSE_THRUST=3|ABORT_ANGLE0=30|OPPOSE_MOTION=1.0:0.5)
#feet hold lower legs to initial angle 
#MAX_THRUST on left foot motor can now increase to push body down onto right foot
#left foot needs to tilt body forward  (LLLX0 is around -12)
#5,MotorAccel(MOTOR_LEFT_FOOT,5,3000,ACCEL_LEFT_FOOT,X,0,-2,2,ACCEL_LEFT_LOWER_LEG,X,LLLX0,-1,1,WAIT_STAGE|HOLD_ANGLE|FUTURE_LOG|MAX_THRUST=8)
#LFM bring LLX from -12/-15 to -20
#5,MotorAccel(MOTOR_LEFT_FOOT,16,3000,ACCEL_LEFT_FOOT,X,0,-4,4,ACCEL_LEFT_LOWER_LEG,X,-20,-1,1,WAIT_STAGE|HOLD_ANGLE|FUTURE_LOG|MAX_THRUST=14)
#LFM seek to stop all motion, biased to falling forward 
5,MotorAccel(MOTOR_LEFT_FOOT,16,3000,ACCEL_LEFT_LOWER_LEG,X,-20,-1,1,WAIT_STAGE|HOLD_ANGLE|FUTURE_LOG|MAX_THRUST=14|OPPOSE_MOTION=0.7:0.4)
#right foot needs to bring RFX to 0
#5,MotorAccel(MOTOR_RIGHT_FOOT,16,3000,ACCEL_RIGHT_FOOT,X,0,-4,4,ACCEL_RIGHT_LOWER_LEG,X,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=14)
#RFM oppose all motion
5,MotorAccel(MOTOR_RIGHT_FOOT,16,3000,ACCEL_RIGHT_LOWER_LEG,X,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=14|OPPOSE_MOTION=0.7:0.4)
#torso oppose motion, bias forward (against positive)
5,MotorAccel(MOTOR_TORSO,3,3000,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=10|MAX_REVERSE_THRUST=2|MAX_VELOCITY=10|OPPOSE_MOTION=0.05:0.15)

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

10000,StopLogging

