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

#legs spread 22cm from left foot to right foot center


#lean left 
#ankle motors
#turn ankles until UPPER_LEFT_LEG reaches 0 degrees
0,MotorAccel(MOTOR_LEFT_ANKLE,1,5000,ACCEL_LEFT_UPPER_LEG,Z,-1,-0.5,0.5,WAIT_STAGE|DOUBLE_THRUST|HOLD_ANGLE)
0,MotorAccel(MOTOR_RIGHT_ANKLE,1,5000,ACCEL_LEFT_UPPER_LEG,Z,-1,-0.5,0.5,WAIT_STAGE|DOUBLE_THRUST)
#keep ankles from turning feet too far
0,MotorAccel(MOTOR_LEFT_ANKLE,1,5000,ACCEL_LEFT_FOOT,Z,0,-3,3,WAIT_STAGE|DOUBLE_THRUST|HOLD_ANGLE)
0,MotorAccel(MOTOR_RIGHT_ANKLE,1,5000,ACCEL_RIGHT_FOOT,Z,0,-3,3,WAIT_STAGE|DOUBLE_THRUST)
#side motors  |MAXTHRUST=2 |MINTHRUST
0,MotorAccel(MOTOR_LEFT_SIDE,2,5000,ACCEL_LEFT_UPPER_LEG,Z,-1,-0.5,0.5,WAIT_STAGE|DOUBLE_THRUST|MOTION_LOG|FUTURE_LOG|HOLD_ANGLE)
0,MotorAccel(MOTOR_RIGHT_SIDE,2,5000,ACCEL_LEFT_UPPER_LEG,Z,-1,-0.5,0.5,WAIT_STAGE|DOUBLE_THRUST)
#legs hold torso X angle
0,MotorAccel(MOTOR_LEFT_LEG,1,5000,ACCEL_TORSO,X,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE|DOUBLE_THRUST)
0,MotorAccel(MOTOR_RIGHT_LEG,1,5000,ACCEL_TORSO,X,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE|DOUBLE_THRUST)
#feet need to hold lower legs
0,MotorAccel(MOTOR_LEFT_FOOT,1,5000,ACCEL_LEFT_LOWER_LEG,X,-12,-1,1,WAIT_STAGE|HOLD_ANGLE)
0,MotorAccel(MOTOR_RIGHT_FOOT,1,5000,ACCEL_RIGHT_LOWER_LEG,X,-12,-1,1,WAIT_STAGE|HOLD_ANGLE)

#step up with right leg
500,MotorAccel(MOTOR_RIGHT_LEG,4,5000,ACCEL_RIGHT_UPPER_LEG,X,8,-0.5,0.5,WAIT_STAGE|HOLD_ANGLE)
500,MotorAccel(MOTOR_RIGHT_KNEE,1,5000,ACCEL_RIGHT_LOWER_LEG,X,CurrentAngle,-0.5,0.5,WAIT_STAGE|HOLD_ANGLE)
500,MotorAccel(MOTOR_RIGHT_FOOT,1,5000,ACCEL_RIGHT_FOOT,X,0,-1,1,WAIT_STAGE|HOLD_ANGLE)
#(counter balance with left leg to hold up torso)
#left leg hold up torso X
500,MotorAccel(MOTOR_LEFT_LEG,1,5000,ACCEL_TORSO,X,-7,-1,1,WAIT_STAGE|HOLD_ANGLE)
#left foot needs to hold left lower leg
500,MotorAccel(MOTOR_LEFT_FOOT,1,5000,ACCEL_LEFT_LOWER_LEG,X,-12,-1,1,WAIT_STAGE|HOLD_ANGLE|FUTURE_LOG)
500,MotorAccel(MOTOR_LEFT_FOOT,1,5000,ACCEL_LEFT_FOOT,X,0,-3,3,WAIT_STAGE|HOLD_ANGLE)


#step back down
#1000,MotorAccel(MOTOR_RIGHT_LEG,1,5000,ACCEL_RIGHT_UPPER_LEG,X,5,-0.5,0.5,WAIT_STAGE)
#1000,MotorAccel(MOTOR_RIGHT_KNEE,1,5000,ACCEL_RIGHT_LOWER_LEG,X,CurrentAngle,-0.5,0.5,WAIT_STAGE|HOLD_ANGLE)
#1000,MotorAccel(MOTOR_RIGHT_FOOT,1,5000,ACCEL_RIGHT_FOOT,X,0,-1,1,WAIT_STAGE|HOLD_ANGLE)
#left leg hold up torso X
#1000,MotorAccel(MOTOR_LEFT_LEG,1,5000,ACCEL_TORSO,X,-7,-1,1,WAIT_STAGE|HOLD_ANGLE)
#left foot needs to hold left lower leg
#1000,MotorAccel(MOTOR_LEFT_FOOT,1,5000,ACCEL_LEFT_LOWER_LEG,X,-12,-1,1,WAIT_STAGE|HOLD_ANGLE)
#1000,MotorAccel(MOTOR_LEFT_FOOT,1,5000,ACCEL_LEFT_FOOT,X,0,-3,3,WAIT_STAGE|HOLD_ANGLE)


#lean back to center - move to 1000 stage?
#ankle motors
#1500,MotorAccel(MOTOR_LEFT_ANKLE,1,5000,ACCEL_LEFT_FOOT,Z,0,-1,1,WAIT_STAGE|DOUBLE_THRUST)
#1500,MotorAccel(MOTOR_RIGHT_ANKLE,1,5000,ACCEL_RIGHT_FOOT,Z,0,-1,1,WAIT_STAGE|DOUBLE_THRUST)
#left leg hold up torso X
#1500,MotorAccel(MOTOR_LEFT_LEG,1,5000,ACCEL_TORSO,X,-7,-1,1,WAIT_STAGE|HOLD_ANGLE)
#side motors #|MAXTHRUST=2
#1500,MotorAccel(MOTOR_LEFT_SIDE,2,5000,ACCEL_LEFT_UPPER_LEG,Z,3,-0.5,0.5,WAIT_STAGE|DOUBLE_THRUST)
#1500,MotorAccel(MOTOR_RIGHT_SIDE,2,5000,ACCEL_LEFT_UPPER_LEG,Z,3,-0.5,0.5,WAIT_STAGE|DOUBLE_THRUST)
#left foot needs to hold left lower leg
#1500,MotorAccel(MOTOR_LEFT_FOOT,1,5000,ACCEL_LEFT_LOWER_LEG,X,-12,-1,1,WAIT_STAGE|HOLD_ANGLE)
#1500,MotorAccel(MOTOR_LEFT_FOOT,1,5000,ACCEL_LEFT_FOOT,X,0,-3,3,WAIT_STAGE|HOLD_ANGLE)


5100,StopLogging

