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

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

#constant thrust step right
#no thrust analysis, but does check for angle


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_RIGHT_UPPER_LEG,X,RULX0)
0,StoreAngle(ACCEL_RIGHT_LOWER_LEG,X,RLLX)
0,StoreAngle(ACCEL_RIGHT_UPPER_LEG,Z,RULZ0)

#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
1,MotorAccel(MOTOR_LEFT_SIDE,MOTOR_RIGHT_SIDE,3,5000,ACCEL_LEFT_LOWER_LEG,Z,-4,-2,1,WAIT_STAGE|DOUBLE_THRUST|FUTURE_LOG|CONSTANT_THRUST)
#1,MotorAccel(MOTOR_LEFT_SIDE,MOTOR_RIGHT_SIDE,3,5000,ACCEL_LEFT_LOWER_LEG,Z,-4,-1,1,WAIT_STAGE|DOUBLE_THRUST|FUTURE_LOG|CONSTANT_THRUST)
1,MotorAccel(MOTOR_LEFT_ANKLE,3,5000,ACCEL_LEFT_FOOT,Z,0,-1,1,ACCEL_LEFT_LOWER_LEG,Z,-4,-2,1,WAIT_STAGE|HOLD_ANGLE|FUTURE_LOG|MAX_THRUST=3)
1,MotorAccel(MOTOR_RIGHT_ANKLE,3,5000,ACCEL_RIGHT_FOOT,Z,0,-1,1,ACCEL_LEFT_LOWER_LEG,Z,-4,-2,1,WAIT_STAGE|HOLD_ANGLE|FUTURE_LOG|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,10000,ACCEL_TORSO,X,TX0,-2,2,WAIT_STAGE|HOLD_ANGLE|FUTURE_LOG|MAX_THRUST=8|MAX_REVERSE_THRUST=3|LAST_STAGE=4)
#1,MotorAccel(MOTOR_LEFT_LEG,MOTOR_RIGHT_LEG,3,5000,ACCEL_TORSO,X,TX0,-1,1,WAIT_STAGE|HOLD_ANGLE|DOUBLE_THRUST|FUTURE_LOG|MAX_THRUST=3)
#1,MotorAccel(MOTOR_LEFT_KNEE,MOTOR_RIGHT_KNEE,3,5000,ACCEL_TORSO,X,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE|DOUBLE_THRUST|FUTURE_LOG|MAX_THRUST=3)
#1,MotorAccel(MOTOR_LEFT_FOOT,3,5000,ACCEL_LEFT_LOWER_LEG,X,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE|FUTURE_LOG|MAX_THRUST=3)
#1,MotorAccel(MOTOR_RIGHT_FOOT,3,5000,ACCEL_RIGHT_LOWER_LEG,X,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE|FUTURE_LOG|MAX_THRUST=3)



#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)
2,MotorAccel(MOTOR_LEFT_SIDE,6,3000,ACCEL_LEFT_LOWER_LEG,Z,-4,-4,1,ACCEL_TORSO,Z,-2,-3,1,WAIT_STAGE|CONSTANT_THRUST|MAX_THRUST=9|ABORT_ON_TIMEOUT)

#ABORT_ANGLE0=5 - end script if ankle Z turns past 5 degrees was -6
2,MotorAccel(MOTOR_LEFT_ANKLE,3,10000,ACCEL_LEFT_FOOT,Z,0,-10,10,ACCEL_LEFT_LOWER_LEG,Z,-6,-1,2,WAIT_STAGE|HOLD_ANGLE|FUTURE_LOG|MAX_THRUST=12|MAX_REVERSE_THRUST=3|ABORT_ANGLE0=15)
#2,MotorAccel(MOTOR_LEFT_ANKLE,3,10000,ACCEL_LEFT_FOOT,Z,0,-10,10,ACCEL_LEFT_LOWER_LEG,Z,-6,-1,2,WAIT_STAGE|HOLD_ANGLE|FUTURE_LOG|MAX_THRUST=12|MAX_REVERSE_THRUST=3|ABORT_ANGLE0=15|LAST_STAGE=4)
#2,MotorAccel(MOTOR_RIGHT_ANKLE,3,3000,ACCEL_RIGHT_FOOT,Z,0,-10,10,ACCEL_LEFT_LOWER_LEG,Z,-5,-2,1,WAIT_STAGE|HOLD_ANGLE|FUTURE_LOG|MAX_THRUST=3)

#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|FUTURE_LOG|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=12|MAX_REVERSE_THRUST=6|LAST_STAGE=4|ABORT_ANGLE0=25)
2,MotorAccel(MOTOR_RIGHT_KNEE,3,3000,ACCEL_RIGHT_UPPER_LEG,X,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=10)

#feet hold up lower legs
2,MotorAccel(MOTOR_LEFT_FOOT,3,3000,ACCEL_LEFT_LOWER_LEG,X,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=10)
2,MotorAccel(MOTOR_RIGHT_FOOT,3,3000,ACCEL_RIGHT_LOWER_LEG,X,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=10)


#2,MotorAccel(MOTOR_LEFT_ANKLE,3,5000,ACCEL_LEFT_FOOT,Z,0,-1,1,ACCEL_LEFT_LOWER_LEG,Z,-4,-1,1,WAIT_STAGE|HOLD_ANGLE|FUTURE_LOG|MAX_THRUST=3)
#2,MotorAccel(MOTOR_RIGHT_SIDE,3,7000,ACCEL_RIGHT_UPPER_LEG,Z,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE|FUTURE_LOG|MAX_THRUST=6)


#Stage 3: LIFT RIGHT LEG only (no knee or foot movement) 
#(LEFT FOOT MOTOR HOLDS ROBOT FROM FALLING FORWARD)
3,MotorAccel(MOTOR_RIGHT_LEG,10,2000,ACCEL_RIGHT_UPPER_LEG,X,17,-1,1,WAIT_STAGE|CONSTANT_THRUST)
3,MotorAccel(MOTOR_LEFT_LEG,10,2000,ACCEL_RIGHT_UPPER_LEG,X,17,-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,10,2000,ACCEL_RIGHT_LOWER_LEG,X,CurrentAnglePlus,5,-1,1,WAIT_STAGE|FUTURE_LOG|CONSTANT_THRUST|HOLD_ANGLE)
3,MotorAccel(MOTOR_RIGHT_KNEE,12,2000,ACCEL_RIGHT_LOWER_LEG,X,-20,-1,1,WAIT_STAGE|FUTURE_LOG|CONSTANT_THRUST|HOLD_ANGLE)


#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,-6,-1,1,WAIT_STAGE|HOLD_ANGLE|FUTURE_LOG|MAX_THRUST=12|MAX_REVERSE_THRUST=3|ABORT_ANGLE0=15|LAST_STAGE=4)


#important that left knee hold ULX to 17 standing is 10 - left knee needs to push hard initially
#3,MotorAccel(MOTOR_LEFT_KNEE,10,2000,ACCEL_LEFT_UPPER_LEG,X,LULX,-0.5,0.5,WAIT_STAGE|HOLD_ANGLE)
#important the left side holds torso Z to 0 (changed to CurrentAngle) was MAX_THRUST=6
#3,MotorAccel(MOTOR_LEFT_SIDE,6,2000,ACCEL_TORSO,Z,0,-2,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=9)
3,MotorAccel(MOTOR_LEFT_SIDE,6,2000,ACCEL_LEFT_LOWER_LEG,Z,-6,-1,1,ACCEL_TORSO,Z,-2,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=9)
#3,MotorAccel(MOTOR_LEFT_SIDE,6,2000,ACCEL_TORSO,Z,-2,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=9)
#important that left ankle holds lower leg Z or else robot falls right  setting Z=-4 forces ankle to thrust immediately
#3,MotorAccel(MOTOR_LEFT_ANKLE,10,2000,ACCEL_LEFT_FOOT,Z,0,-5,5,ACCEL_LEFT_LOWER_LEG,Z,-4,-1,1,WAIT_STAGE|HOLD_ANGLE|FUTURE_LOG|MAX_THRUST=12)
#3,MotorAccel(MOTOR_LEFT_ANKLE,10,2000,ACCEL_LEFT_LOWER_LEG,Z,-4,-1,1,WAIT_STAGE|HOLD_ANGLE|FUTURE_LOG|MAX_THRUST=16|LAST_STAGE=4|ABORT_ANGLE0=10)
#ankle inst lasts through balance stage (4)
#3,MotorAccel(MOTOR_LEFT_ANKLE,10,2000,ACCEL_LEFT_FOOT,Z,0,-10,10,ACCEL_LEFT_LOWER_LEG,Z,-4,-1,1,WAIT_STAGE|HOLD_ANGLE|FUTURE_LOG|MAX_THRUST=16|LAST_STAGE=4|ABORT_ANGLE0=15)
#torso motor holds up torso X
#3,MotorAccel(MOTOR_TORSO,9,2000,ACCEL_TORSO,X,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE|FUTURE_LOG)
#right side pushes away from left leg
3,MotorAccel(MOTOR_RIGHT_SIDE,6,2000,ACCEL_RIGHT_UPPER_LEG,Z,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE)
#3,MotorAccel(MOTOR_RIGHT_SIDE,6,2000,ACCEL_TORSO,Z,0,-1,1,WAIT_STAGE|HOLD_ANGLE)
#left foot holds lower leg to current angle 
3,MotorAccel(MOTOR_LEFT_FOOT,5,2000,ACCEL_LEFT_FOOT,X,0,-2,2,ACCEL_LEFT_LOWER_LEG,X,CurrentAngle[LLLX],-1,1,WAIT_STAGE|HOLD_ANGLE|FUTURE_LOG|ABORT_ANGLE1=-30)
#left ankle holds robot weight from falling left
#2,MotorAccel(MOTOR_LEFT_ANKLE,10,6100,ACCEL_LEFT_FOOT,Z,0,-5,5,ACCEL_LEFT_LOWER_LEG,Z,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=16|FUTURE_LOG|LAST_STAGE=6100)

#right ankle hold Z at 0
3,MotorAccel(MOTOR_RIGHT_ANKLE,10,2000,ACCEL_RIGHT_FOOT,Z,0,-3,3,WAIT_STAGE|FUTURE_LOG|HOLD_ANGLE)


#right foot bends up to lower friction of foot with ground
#3,MotorAccel(MOTOR_RIGHT_FOOT,6,2000,ACCEL_RIGHT_FOOT,X,8,-0.5,0.5,WAIT_STAGE|FUTURE_LOG|HOLD_ANGLE)


#right foot holds foot parallel to ground X=0 (standing should be x=0, but can be ex: 2)
#3,MotorAccel(MOTOR_RIGHT_FOOT,6,2000,ACCEL_RIGHT_FOOT,X,0,-0.5,0.5,WAIT_STAGE|FUTURE_LOG|HOLD_ANGLE)


#Stage 3: LOWER RIGHT LEG
#3,MotorAccel(MOTOR_RIGHT_LEG,6,400,ACCEL_RIGHT_UPPER_LEG,X,RULX0,-0.5,0.5,WAIT_STAGE|CONSTANT_THRUST)
#3,MotorAccel(MOTOR_RIGHT_KNEE,9,400,ACCEL_RIGHT_LOWER_LEG,X,RLLX,-0.5,0.5,WAIT_STAGE|CONSTANT_THRUST|HOLD_ANGLE)
#3,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
#3,MotorAccel(MOTOR_LEFT_ANKLE,10,800,ACCEL_LEFT_FOOT,Z,0,-5,5,ACCEL_LEFT_LOWER_LEG,Z,LLLZ0,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=16|FUTURE_LOG|LAST_STAGE=6100)


#Stage 4: BALANCE - hole right leg up in the air for 3 seconds
#NOTE: Ankle in stage 2 holds until stage 4
#(Basically try to hold the current angles)
4,MotorAccel(MOTOR_LEFT_LEG,10,3000,ACCEL_TORSO,X,TX0,-1,1,WAIT_STAGE|HOLD_ANGLE)
#4,MotorAccel(MOTOR_LEFT_KNEE,10,3000,ACCEL_LEFT_UPPER_LEG,X,LULX,-0.5,0.5,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=14)
#important the left side holds torso Z to 0 (changed to CurrentAngle)
#the torso going ccw to help left lower leg (brings torso and left leg together, instead of apart=cw or together to the right) causes a fall right
#4,MotorAccel(MOTOR_LEFT_SIDE,3,3000,ACCEL_LEFT_LOWER_LEG,Z,-6,-2,1,ACCEL_TORSO,Z,-2,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=10)
4,MotorAccel(MOTOR_LEFT_SIDE,3,3000,ACCEL_TORSO,Z,-2,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=10)
#important that left ankle holds lower leg Z or else robot falls right  setting Z=-4 forces ankle to thrust immediately
#4,MotorAccel(MOTOR_LEFT_ANKLE,5,3000,ACCEL_LEFT_LOWER_LEG,Z,-4,-1,1,WAIT_STAGE|HOLD_ANGLE|FUTURE_LOG|MAX_THRUST=16|ABORT_ANGLE0=10)
#torso motor holds up torso X
#4,MotorAccel(MOTOR_TORSO,9,2000,ACCEL_TORSO,X,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE|FUTURE_LOG)
#right side pushes away from left leg
#4,MotorAccel(MOTOR_RIGHT_SIDE,6,2000,ACCEL_RIGHT_UPPER_LEG,Z,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE)
#left foot holds lower leg to current angle 
4,MotorAccel(MOTOR_LEFT_FOOT,5,3000,ACCEL_LEFT_FOOT,X,0,-2,2,ACCEL_LEFT_LOWER_LEG,X,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE|FUTURE_LOG)
#left ankle holds robot weight from falling left
#2,MotorAccel(MOTOR_LEFT_ANKLE,10,6100,ACCEL_LEFT_FOOT,Z,0,-5,5,ACCEL_LEFT_LOWER_LEG,Z,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE|MAX_THRUST=16|FUTURE_LOG|LAST_STAGE=6100)
#torso motor holds up torso X
#4,MotorAccel(MOTOR_TORSO,3,2000,ACCEL_TORSO,X,CurrentAngle,-1,1,WAIT_STAGE|HOLD_ANGLE|FUTURE_LOG|MAX_THRUST=4|MAX_REVERSE_THRUST=3)



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

10000,StopLogging

