I’d be happy to run though it!
I’m pulling the latest code from the kinematics off GitHub, so this is as of where it stands right now. I cleaned up a little of the formatting on the way.
First, the distance from the center of each motor axis to the center of the cutting bit is determined. This is based on the coordinates of the motors, determined by either measurements or the calibration process, and uses the target coordinates for the bit:
Motor1Distance = sqrt((-1*_xCordOfMotor - xTarget)^2 + (_yCordOfMotor - yTarget)^2)
Motor2Distance = sqrt((_xCordOfMotor - xTarget)^2 + (_yCordOfMotor - yTarget)^2)
Next, we determine what the angles of the chains are from the motors to the cutting bit, as well as how much chain is wrapped around the sprocket. Note that the chain length technically begins at the 12 o’clock position on the sprocket, so some chain will always be wrapped around the sprocket.
Also note that this now depends on if the chain exits the sprocket to go to the sled from the top or bottom of the sprocket. Something to look forward to in the next release!
If the chain exits from the top of the sprocket (conventional design):
Chain1Angle = asin((_yCordOfMotor - yTarget)/Motor1Distance) + asin(R/Motor1Distance)
Chain2Angle = asin((_yCordOfMotor - yTarget)/Motor2Distance) + asin(R/Motor2Distance)
Chain1AroundSprocket = R * Chain1Angle
Chain2AroundSprocket = R * Chain2Angle
If the chain exits from the bottom of the sprocket (new possible configuration):
Chain1Angle = asin((_yCordOfMotor - yTarget)/Motor1Distance) - asin(R/Motor1Distance)
Chain2Angle = asin((_yCordOfMotor - yTarget)/Motor2Distance) - asin(R/Motor2Distance)
Chain1AroundSprocket = R * (3.14159 - Chain1Angle)
Chain2AroundSprocket = R * (3.14159 - Chain2Angle)
Next we calculate the straight length of chain from the motors to the bit. Note here that the chain does not actually come from the center of the motor, as so far we have been calculating. It actually comes from the tangential spot on the sprocket, so we account for that in this step:
Chain1Straight = sqrt(Motor1Distance^2 - R^2)
Chain2Straight = sqrt(Motor2Distance^2 - R^2)
This next step is a bit more complicated. Here, we account and correct for chain sag in the straight portion of the chain. To do this, we use the chainSagCorrection value as part of the equation. The chainSagCorrection value is a combined coefficient, and includes the weight of the sled, among other factors. This step works by determining the percentage error of the straight line chain length, based on the chain angle, chain length, and chain tension, respectively, and then adding that percentage error into the chain length to compensate:
Chain1Straight *= 1 + ((chainSagCorrection / 1000000000000) * cos(Chain1Angle)^2 * Chain1Straight^2 * ((tan(Chain2Angle) * cos(Chain1Angle)) + sin(Chain1Angle))^2)
Chain2Straight *= 1 + ((chainSagCorrection / 1000000000000) * cos(Chain2Angle)^2 * Chain2Straight^2 * ((tan(Chain1Angle) * cos(Chain2Angle)) + sin(Chain2Angle))^2)
Next, we add the chain length wrapped around the sprocket to the straight chain length:
Chain1 = Chain1AroundSprocket + Chain1Straight
Chain2 = Chain2AroundSprocket + Chain2Straight
And finally we subtract the distance added by the linkage rotation mechanism:
Chain1 = Chain1 - rotationDiskRadius
Chain2 = Chain2 - rotationDiskRadius
That’s it in a nutshell!