#RightStepForward03.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)
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,4,5000,ACCEL_LEFT_LOWER_LEG,Z,-5,-1,1,WAIT_STAGE|DOUBLE_THRUST|MAX_VELOCITY=10|CONSTANT_THRUST)
#1,MotorAccel(MOTOR_RIGHT_SIDE,4,5000,ACCEL_LEFT_LOWER_LEG,Z,-5,-1,1,WAIT_STAGE|HOLD_ANGLE|DOUBLE_THRUST|MAX_VELOCITY=10|CONSTANT_THRUST)
#1,MotorAccel(MOTOR_LEFT_ANKLE,3,5000,ACCEL_LEFT_FOOT,Z,0,-5,5,ACCEL_LEFT_LOWER_LEG,Z,-5,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=3|MAX_REVERSE_THRUST=4|MAX_VELOCITY=10|FUTURE_LOG|MAINTAIN_THRUST)
#1,MotorAccel(MOTOR_RIGHT_ANKLE,3,5000,ACCEL_RIGHT_FOOT,Z,0,-5,5,ACCEL_LEFT_LOWER_LEG,Z,-5,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=3|MAX_REVERSE_THRUST=4|MAX_VELOCITY=10)
##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)
#ok to have a large -offsetdegrees because the motor direction is the same to bring the lower leg positive, and the torso will not complete if the lower leg is out of range otherwise 
2,MotorAccel(MOTOR_LEFT_SIDE,3,4000,ACCEL_LEFT_LOWER_LEG,Z,-5,-6,1,ACCEL_TORSO,Z,-2,-5,1,WAIT_STAGE|CONSTANT_THRUST|MAX_THRUST=12|MAINTAIN_THRUST|ABORT_ON_TIMEOUT|MAX_VELOCITY=20)
#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,-10,10,ACCEL_LEFT_LOWER_LEG,Z,-5,-2,0,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=18|MAX_REVERSE_THRUST=10|MAX_VELOCITY=5|FUTURE_LOG|ABORT_ANGLE0=20|MAINTAIN_THRUST)
#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)


#Stage 3: LIFT RIGHT LEG only (no knee or foot movement) 
#(LEFT FOOT MOTOR HOLDS ROBOT FROM FALLING FORWARD)
3,MotorAccel(MOTOR_RIGHT_LEG,16,600,ACCEL_RIGHT_UPPER_LEG,X,20,-1,1,WAIT_STAGE|CONSTANT_THRUST|MAINTAIN_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)
3,MotorAccel(MOTOR_LEFT_KNEE,3,600,ACCEL_LEFT_UPPER_LEG,X,LULX,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=9|MAX_REVERSE_THRUST=6|MAINTAIN_THRUST|ABORT_ANGLE0=25)
#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=18|MAX_REVERSE_THRUST=10|MAX_VELOCITY=5|ABORT_ANGLE0=20|MAINTAIN_THRUST|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)
3,MotorAccel(MOTOR_LEFT_SIDE,9,600,ACCEL_TORSO,Z,-2,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=12|MAX_REVERSE_THRUST=2|MAINTAIN_THRUST)
#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)
3,MotorAccel(MOTOR_RIGHT_SIDE,1,600,ACCEL_RIGHT_LOWER_LEG,Z,-12,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=14|MAX_REVERSE_THRUST=4|MAINTAIN_THRUST)
#left foot holds lower leg to current angle 
3,MotorAccel(MOTOR_LEFT_FOOT,5,600,ACCEL_LEFT_FOOT,X,0,-2,2,ACCEL_LEFT_LOWER_LEG,X,CurrentAngle[LLLX],-1,1,WAIT_STAGE|HOLD_ANGLE|ABORT_ANGLE1=-30|MAX_THRUST=12|MAX_REVERSE_THRUST=3|MAX_VELOCITY=5)
#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=15|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20|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 4: LOWER RIGHT LEG SWINGS FORWARD, left side holds right leg off the ground 
#right knee swings forward 
4,MotorAccel(MOTOR_RIGHT_KNEE,12,400,ACCEL_RIGHT_LOWER_LEG,X,0,-1,1,WAIT_STAGE|CONSTANT_THRUST)
#4,MotorAccel(MOTOR_RIGHT_FOOT,1,400,ACCEL_RIGHT_FOOT,X,0,-1,1,WAIT_STAGE|HOLD_ANGLE) #does not use constant thrust
#left ankle leans back
4,MotorAccel(MOTOR_LEFT_ANKLE,3,400,ACCEL_LEFT_FOOT,Z,0,-15,15,ACCEL_LEFT_LOWER_LEG,Z,-7,-2,0,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=14|MAX_REVERSE_THRUST=3|ABORT_ANGLE0=20|MAINTAIN_THRUST)
#4,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)
#Note there is a problem in stage 4 when the LLLZ starts to move back to the right (positive roll), the LeftSide motor needs to keep lifting the right leg, to reverse would quickly bring the right leg down. So for now, just link the LeftSide motor to the Torso Z 
4,MotorAccel(MOTOR_LEFT_SIDE,6,400,ACCEL_TORSO,Z,-2,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=11|MAX_REVERSE_THRUST=2|MAINTAIN_THRUST)
#4,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)
4,MotorAccel(MOTOR_RIGHT_SIDE,1,400,ACCEL_RIGHT_LOWER_LEG,Z,-12,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=10|MAX_REVERSE_THRUST=2|MAINTAIN_THRUST)
4,MotorAccel(MOTOR_TORSO,3,400,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=8|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20|MAINTAIN_THRUST)
#4,MotorAccel(MOTOR_LEFT_LEG,10,2000,ACCEL_RIGHT_UPPER_LEG,X,17,-1,1,WAIT_STAGE|MAINTAIN_THRUST|MAX_THRUST=8|MAX_REVERSE_THRUST=3|HOLD_ANGLE)
#Left Leg Motor holds hip (or torso)
#4,MotorAccel(MOTOR_LEFT_LEG,10,400,ACCEL_HIP,X,HX0,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=8|MAX_REVERSE_THRUST=3|MAINTAIN_THRUST)
4,MotorAccel(MOTOR_LEFT_LEG,10,600,ACCEL_RIGHT_UPPER_LEG,X,20,-1,1,WAIT_STAGE|CONSTANT_THRUST|HOLD_ANGLE)
#right leg motor holds up right leg so lower leg and foot can clear in swinging right lower leg forward
4,MotorAccel(MOTOR_RIGHT_LEG,10,600,ACCEL_RIGHT_UPPER_LEG,X,20,-1,1,WAIT_STAGE|CONSTANT_THRUST|HOLD_ANGLE)
4,MotorAccel(MOTOR_LEFT_FOOT,5,400,ACCEL_LEFT_FOOT,X,0,-5,5,ACCEL_LEFT_LOWER_LEG,X,LLLX0,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=12|MAX_REVERSE_THRUST=3|MAX_VELOCITY=10)
#|ABORT_ANGLE1=-30  make -2 2 range?
4,MotorAccel(MOTOR_LEFT_KNEE,3,400,ACCEL_LEFT_UPPER_LEG,X,LULX,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=12|MAX_REVERSE_THRUST=6|MAINTAIN_THRUST|ABORT_ANGLE0=25)


#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)
#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)
#legs - both try to hold torso X to starting angle
5,MotorAccel(MOTOR_LEFT_LEG,MOTOR_RIGHT_LEG,3,3000,ACCEL_TORSO,X,TX0,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=8|MAX_REVERSE_THRUST=3|DOUBLE_THRUST)
#knees - for now hold the current X based on the upper leg X
#5,MotorAccel(MOTOR_RIGHT_KNEE,9,3000,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-1,1,WAIT_STAGE|CONSTANT_THRUST|HOLD_ANGLE)
#5,MotorAccel(MOTOR_LEFT_KNEE,9,3000,ACCEL_LEFT_UPPER_LEG,X,LULX0,-1,1,WAIT_STAGE|CONSTANT_THRUST|HOLD_ANGLE)
5,MotorAccel(MOTOR_RIGHT_KNEE,9,3000,ACCEL_RIGHT_UPPER_LEG,X,CurrentAngle,-1,1,WAIT_STAGE|CONSTANT_THRUST|HOLD_ANGLE)
5,MotorAccel(MOTOR_LEFT_KNEE,9,3000,ACCEL_LEFT_UPPER_LEG,X,CurrentAngle,-1,1,WAIT_STAGE|CONSTANT_THRUST|HOLD_ANGLE)
#left ankle needs to go back to the original angle - based on left lower leg Z
5,MotorAccel(MOTOR_LEFT_ANKLE,3,3000,ACCEL_LEFT_FOOT,Z,0,-10,10,ACCEL_LEFT_LOWER_LEG,Z,LLLZ0,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=12|MAX_REVERSE_THRUST=3|ABORT_ANGLE0=20)
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=12|MAX_REVERSE_THRUST=3|ABORT_ANGLE0=20)
#feet hold lower legs to initial angle 
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)
5,MotorAccel(MOTOR_RIGHT_FOOT,5,3000,ACCEL_RIGHT_FOOT,X,0,-2,2,ACCEL_RIGHT_LOWER_LEG,X,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE|FUTURE_LOG)
5,MotorAccel(MOTOR_TORSO,3,3000,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=4|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20)

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

10000,StopLogging

