FastCal.make_ins

; This calibration does all the joints at once. 
; This can cause vibrations, especially in joints 4 and 5, which cause the cailbration to be "jerky"
w, 42, 832; COMMAND_REG = AClrLoop, ResetMotorPosition, ResetForce
w, 42, 512; COMMAND_REG = ResetForce
w, 79, 86; PID Timebase Divisor = 86
w, 80, 86; Physics J123 Timebase Divisor = 86
w, 81, 172; Physics J45 Timebase Divisor = 172
S, J1BoundryHigh, 670010;
S, J1BoundryLow, -670010;
S, J2BoundryLow, -350010;
S, J2BoundryHigh, 350010;
S, J3BoundryLow, -570010;
S, J3BoundryHigh, 570010;
S, J4BoundryLow, -390010;
S, J4BoundryHigh, 390010;
S, J5BoundryLow, -684010;
S, J5BoundryHigh, 684010;
S, MaxSpeed, 300000;
a, 0, 0, 0, 0, 0;
a, 648000, 360000, -594000, 396000, 648000;
F; Wait for move to complete
S, MaxSpeed, 280000; Smoothest known speed
w, 42, 3079; COMMAND_REG = CapCalibrateBase, CapCalibrateEnd, CapCalibratePivot, CapCalAngle, CapCalRot
a, -648000, -360000, 594000, -396000, -648000;
F; Wait for smooth calibration sweep to complete
w, 42, 0; COMMAND_REG = 0
S, MaxSpeed, 300000;
a, 0, 0, 0, 0, 0;
a, 20000, 20000, 20000, 20000, 20000;
a, 0, 0, 0, 0, 0;
F; Wait for FPGA tables to load
S, J1BoundryHigh, 540000;
S, J1BoundryLow, -540000;
S, J2BoundryHigh, 324000;
S, J2BoundryLow, -324000;
S, J3BoundryHigh, 540000;
S, J3BoundryLow, -540000;
S, J4BoundryHigh, 324000;
S, J4BoundryLow, -324000;
S, J5BoundryHigh, 504000;
S, J5BoundryLow, -504000;
w, 79, 86; PID Timebase Divisor = 86
w, 80, 86; Physics J123 Timebase Divisor = 86
w, 81, 172; Physics J45 Timebase Divisor = 172
w, 52, 5; DIFF_FORCE_BETA	 = 5
w, 28, 5; SPEED_FACTORB = 5 ???
z, 1; Sleep for 1 second
w, 42, 12960; COMMAND_REG = EnableLoop, CAL_RUN, ResetForce, AngleEnable, RotEnable