#Lean_Back_and_Forth_05.sh
0,StartLogging 
0,LogScriptFileName
#StartTimeInMS,HoldAngleWithMotor(AccelName,AccelAngleXYorZ,TargetAngle,DegreesPlus,DegreesMinus,MotorName,Dir*Strength,Duration/TimeOut,flags)


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)


1,StartLoop(-1,Any,31)  #***START LOOP***  Any key ends loop, escape key aborts script
# end loop after loop 2 times, any key to stop loop, escape key to abort loop (and script)


#Stage 1: LEAN LEFT 
#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,2000,ACCEL_LEFT_LOWER_LEG,Z,-4,-2,1,WAIT_STAGE|DOUBLE_THRUST|CONSTANT_THRUST)
1,MotorAccel(MOTOR_LEFT_SIDE,4,2000,ACCEL_LEFT_LOWER_LEG,Z,0,-1,1,WAIT_STAGE|DOUBLE_THRUST|CONSTANT_THRUST|MAX_VELOCITY=5)
1,MotorAccel(MOTOR_RIGHT_SIDE,4,2000,ACCEL_LEFT_LOWER_LEG,Z,0,-1,1,WAIT_STAGE|HOLD_ANGLE|DOUBLE_THRUST|CONSTANT_THRUST|MAX_VELOCITY=5)
1,MotorAccel(MOTOR_LEFT_ANKLE,4,2000,ACCEL_LEFT_FOOT,Z,0,-5,5,ACCEL_LEFT_LOWER_LEG,Z,0,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=3|MAX_REVERSE_THRUST=5|MAX_VELOCITY=5|FUTURE_LOG)
1,MotorAccel(MOTOR_RIGHT_ANKLE,4,2000,ACCEL_RIGHT_FOOT,Z,0,-5,5,ACCEL_LEFT_LOWER_LEG,Z,0,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=3)
#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,20000,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,2000,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=5|MAX_REVERSE_THRUST=3|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,8,2000,ACCEL_LEFT_LOWER_LEG,Z,0,-1,1,ACCEL_TORSO,Z,0,-2,1,WAIT_STAGE|CONSTANT_THRUST|MAX_THRUST=10|MAINTAIN_THRUST|MAX_VELOCITY=10)
#ABORT_ANGLE0=5 - end script if ankle Z turns past 5 degrees was -6
#2,MotorAccel(MOTOR_LEFT_ANKLE,3,2000,ACCEL_LEFT_FOOT,Z,0,-10,10,ACCEL_LEFT_LOWER_LEG,Z,0,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=14|MAX_REVERSE_THRUST=5|MAX_VELOCITY=5|FUTURE_LOG|ABORT_ANGLE0=15) #|MAINTAIN_THRUST
##2,MotorAccel(MOTOR_LEFT_KNEE,3,2000,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,2000,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,2000,ACCEL_LEFT_LOWER_LEG,X,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=10)
##2,MotorAccel(MOTOR_RIGHT_FOOT,3,2000,ACCEL_RIGHT_LOWER_LEG,X,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=10)
#2,MotorAccel(MOTOR_TORSO,3,2000,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=4|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20) #|MAINTAIN_THRUST


#***important that right foot needs to have very little to no weight on it, or else the torso falls back
#3,StoreAngle(ACCEL_LEFT_UPPER_LEG,X,RULX1)
#stage 3: raise right leg
#3,MotorAccel(MOTOR_RIGHT_LEG,8,1000,ACCEL_RIGHT_UPPER_LEG,X,CurrentAnglePlus,3,-1,1,WAIT_STAGE|CONSTANT_THRUST|MAX_THRUST=10|MAINTAIN_THRUST|MAX_VELOCITY=50)
#left leg counters
#3,MotorAccel(MOTOR_LEFT_LEG,8,1000,ACCEL_LEFT_UPPER_LEG,X,CurrentAnglePlus,-3,-1,1,WAIT_STAGE|HOLD_ANGLE|CONSTANT_THRUST|MAX_THRUST=10|MAINTAIN_THRUST|MAX_VELOCITY=50)

#stage 4: lower left leg back to original angle (RULX1)
#4,MotorAccel(MOTOR_LEFT_LEG,8,1000,ACCEL_LEFT_UPPER_LEG,X,RULX1,-1,1,WAIT_STAGE|CONSTANT_THRUST|MAX_THRUST=10|MAINTAIN_THRUST|MAX_VELOCITY=50)
#4,MotorAccel(MOTOR_RIGHT_LEG,8,1000,ACCEL_LEFT_UPPER_LEG,X,RULX1,-1,1,WAIT_STAGE|HOLD_ANGLE|CONSTANT_THRUST|MAX_THRUST=10|MAINTAIN_THRUST|MAX_VELOCITY=50)


#Stage 10: LEAN RIGHT 
#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
#10,MotorAccel(MOTOR_LEFT_SIDE,MOTOR_RIGHT_SIDE,3,2000,ACCEL_LEFT_LOWER_LEG,Z,-4,-2,1,WAIT_STAGE|DOUBLE_THRUST|CONSTANT_THRUST)
10,MotorAccel(MOTOR_LEFT_SIDE,4,2000,ACCEL_RIGHT_LOWER_LEG,Z,0,-1,1,WAIT_STAGE|DOUBLE_THRUST|CONSTANT_THRUST)
10,MotorAccel(MOTOR_RIGHT_SIDE,4,2000,ACCEL_RIGHT_LOWER_LEG,Z,0,-1,1,WAIT_STAGE|HOLD_ANGLE|DOUBLE_THRUST|CONSTANT_THRUST)
10,MotorAccel(MOTOR_LEFT_ANKLE,4,2000,ACCEL_LEFT_FOOT,Z,0,-5,5,ACCEL_RIGHT_LOWER_LEG,Z,0,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=3|MAX_REVERSE_THRUST=5|MAX_VELOCITY=5|FUTURE_LOG)
10,MotorAccel(MOTOR_RIGHT_ANKLE,4,2000,ACCEL_RIGHT_FOOT,Z,0,-5,5,ACCEL_RIGHT_LOWER_LEG,Z,0,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=3)
#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
#10,MotorAccel(MOTOR_TORSO,3,20000,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=4|MAX_REVERSE_THRUST=3|LAST_STAGE=4|MAX_VELOCITY=20)
10,MotorAccel(MOTOR_TORSO,3,2000,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=5|MAX_REVERSE_THRUST=3|MAX_VELOCITY=5)



#stage 11: RIGHT_SIDE reverses, to make torso Z straighten, and put torso weight over left leg, can be more positive than 1deg
#11,MotorAccel(MOTOR_RIGHT_SIDE,8,2000,ACCEL_RIGHT_LOWER_LEG,Z,0,-1,1,ACCEL_TORSO,Z,0,-1,2,WAIT_STAGE|CONSTANT_THRUST|MAX_THRUST=10|MAX_VELOCITY=10) #|MAINTAIN_THRUST
#ABORT_ANGLE0=5 - end script if ankle Z turns past 5 degrees was -6
#ABORT_ON_TIMEOUT|
#11,MotorAccel(MOTOR_RIGHT_ANKLE,3,2000,ACCEL_RIGHT_FOOT,Z,0,-10,10,ACCEL_RIGHT_LOWER_LEG,Z,0,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=14|MAX_REVERSE_THRUST=5|MAX_VELOCITY=5|FUTURE_LOG|ABORT_ANGLE0=15) #|MAINTAIN_THRUST
##11,MotorAccel(MOTOR_LEFT_KNEE,3,2000,ACCEL_LEFT_UPPER_LEG,X,LULX,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=9|MAX_REVERSE_THRUST=6|MAINTAIN_THRUST|ABORT_ANGLE0=25)
##11,MotorAccel(MOTOR_RIGHT_KNEE,3,2000,ACCEL_RIGHT_UPPER_LEG,X,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=8)
#feet hold up lower legs
##11,MotorAccel(MOTOR_LEFT_FOOT,3,2000,ACCEL_LEFT_LOWER_LEG,X,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=10)
##11,MotorAccel(MOTOR_RIGHT_FOOT,3,2000,ACCEL_RIGHT_LOWER_LEG,X,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=10)
#11,MotorAccel(MOTOR_TORSO,3,2000,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=4|MAX_REVERSE_THRUST=3|MAX_VELOCITY=20) #|MAINTAIN_THRUST

20,EndLoop  #  ***END LOOP****

#Stage 9: LEAN CENTER 3,MotorAccel(MOTOR_LEFT_SIDE,3,2000,ACCEL_RIGHT_LOWER_LEG,Z,3.5,-1,1,WAIT_STAGE|DOUBLE_THRUST|CONSTANT_THRUST)
30,MotorAccel(MOTOR_RIGHT_SIDE,3,4000,ACCEL_RIGHT_LOWER_LEG,Z,-3.5,-1,1,WAIT_STAGE|HOLD_ANGLE|DOUBLE_THRUST|CONSTANT_THRUST)
30,MotorAccel(MOTOR_LEFT_SIDE,3,4000,ACCEL_LEFT_LOWER_LEG,Z,3.5,-1,1,WAIT_STAGE|HOLD_ANGLE|DOUBLE_THRUST|CONSTANT_THRUST)
30,MotorAccel(MOTOR_LEFT_ANKLE,3,4000,ACCEL_LEFT_FOOT,Z,0,-5,5,ACCEL_LEFT_LOWER_LEG,Z,3.5,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=3|MAX_REVERSE_THRUST=5|MAX_VELOCITY=5)
30,MotorAccel(MOTOR_RIGHT_ANKLE,3,4000,ACCEL_RIGHT_FOOT,Z,0,-5,5,ACCEL_RIGHT_LOWER_LEG,Z,-3.5,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=3|MAX_REVERSE_THRUST=5|MAX_VELOCITY=5)
30,MotorAccel(MOTOR_TORSO,3,4000,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=5|MAX_REVERSE_THRUST=3|MAX_VELOCITY=5)



8000,StopLogging

