position_orientation = reshape(A,1,); % [nx ny nz ox oy oz ax ay az Px Py Pz]]
% round down small numbers
tolerance = 0.0001;
for i = 1:length(position_orientation)
if abs(position_orientation(i)) < tolerance
position_orientation(i) = 0;
Forward Kinematic Equations:
Trouble Shooting Guide
Although we didn’t experience any major technical difficulties after the arm was built and tested, there are a few aspects of the assembly that may need attention after more use. The overall structure of the arm is pushing the bounds of the TETRIX components. To keep the arm in good working condition the following should be considered.
At full extension, the motors have to resist a torque that is very near their limit. For this reason, motions that are programmed into the arm must be very slow. Quick movements (accelerations) increase the risk of stripping, bending, or loosening components. If you want the arm to move faster or carry a greater load, a first step would be to make it smaller by shortening the tubes.
The chain is tensioned by positioning the tube clamps at the ends of the tubes on the upper arm. When there is load on the arm, the chain will become loose due to flex in the system. This can’t be avoided. If the chain becomes excessively loose, follow these steps to adjust it.
Loosen the set screw that binds the 40T gear to the DC motor shaft that controls the elbow joint. This will make the elbow joint move freely.
Loosen the 4 tubing clamps on the upper arm. Make sure it doesn’t fall apart.
On the non-chain side, slide the clamps away from the center of the tube a couple of millimeters and re-tighten the clamps. See below. The tubing plugs may slide away from the ends of the tube when you loosen the clamps. Make sure you slide them back before you retighten the clamps.
Do the same on the other side. As you move the clamps away, you will feel the chain getting tight. Make the chain as tight as you can without sliding the clamp off the end of the tube. This process it a bit awkward and may take several attempts.
Make sure you don’t rotate the clamps relative to one another. This will put a twist in the arm.
Re-tighten the set screw on the 40T gear. Make sure the screw is against the flat on the motor shaft.
The main shoulder pivot may become loose. Follow these steps to correct the problem.
Loosen the set screws that bind the inside split clamps to the main axle. These are highlighted in the figure below. On the real robot, these pieces have been modifies and don’t look exactly the same as in the figure.
Sandwich these components together and simultaneously tighten the set screws. This will take two people. Try to get at least one setscrew on each clamp to tighten against the flat on the axle.
Now, do the same thing with the axle set collars. See figure below.
My final word of advice is in regard to the supposed position control of the DC motors with RobotC. There is no PID algorithm for position control, only for speed. After reading everything I could find about this on RobotC forums and in the documentation, there does not seem to be a reliable way to command the motors to a position. The main approach that I found was to drive the motor at a constant speed (with the PID speed control) in a while loop until some encoder value was reached. If you want to implement some sophisticated method of motion control with the robot, I don’t think this will cut it. If you read about other’s frustrations with this on the forums, you will see that it hasn’t cut it for anyone else ether. My advice would be to design a controller on your own unless you find successful solution on the internet.