Program a 4-DOF robotic arm

Build the arm and wire its power correctly, with a way to cut power instantly — the foundation that keeps a fun project from becoming a pinched finger or a fried board.

1. 1.1
   Assemble the arm mechanically following its kit instructions, but do NOT screw the servo horns to their final angle yet — you will centre each servo in code first, then attach the horns, so the joints start from a known position.

   Attaching horns before centring is the classic arm-build mistake: the servo then cannot reach half its range. Centre in code, then mount the horn.
2. 1.2
   Wire power deliberately: the servos get their own 5–6 V supply (not the Arduino's 5 V pin), the supply ground is joined to the Arduino ground, and a switch sits on the servo supply's positive lead so you can cut all servo power in an instant. Confirm the supply voltage and common ground with a multimeter before anything moves.

   Check: The servo supply reads 5–6 V, its ground is common with the Arduino, and flipping your switch removes power to the servos.
3. 1.3
   Fix the arm's base down — clamp it or bolt it to a board — so it cannot tip when it swings out. An unsecured arm can topple or drag itself off the bench the first time a joint moves quickly.

   Also clear the arm's workspace: nothing (and nobody's hand) within its reach while it is powered. Treat even a small arm as something that can move fast and unexpectedly during a bug.
4. 1.4
   Write your channel plan into a comment block: which servo/PCA9685 channel is base, shoulder, elbow and gripper, and each one's safe minimum and maximum pulse (so code can never drive a joint past its mechanical limit).

   cppCopy

   // Joint plan (pulse microseconds; tighten these to YOUR arm)
   // 0 base:     min 600  max 2400  home 1500
   // 1 shoulder: min 900  max 2100  home 1500
   // 2 elbow:    min 700  max 2300  home 1500
   // 3 gripper:  open 1900  closed 1200

Bring each servo to life one at a time, discover its real safe range, and define a home pose — always testing slowly so a mistake nudges rather than slams.

1. 2.1
   Initialise the servo driver and write a helper that commands one joint by a microsecond pulse, clamped to that joint's safe min/max so a bad command can never drive it into a hard stop. Clamping in the lowest-level helper protects the arm from every bug above it.

   cppCopy

   #include <Servo.h>
   Servo joints[4];
   const int PIN[4] = {3, 5, 6, 9};
   int jmin[4] = {600, 900, 700, 1200};
   int jmax[4] = {2400, 2100, 2300, 1900};
   
   void setupArm() {
     for (int i = 0; i < 4; i++) joints[i].attach(PIN[i]);
   }
   
   void setJoint(int i, int us) {
     us = constrain(us, jmin[i], jmax[i]);   // never exceed a joint's limit
     joints[i].writeMicroseconds(us);
   }

2. 2.2
   Power on with the arm supported by your hand, and command each joint in turn to its home pulse. Now attach each servo horn so the arm sits in a sensible neutral pose at home. This is the 'centre then mount' step that makes the whole range usable.

   Check: At the home command every joint sits at a natural mid-position and the arm holds a tidy neutral pose.
3. 2.3
   Find each joint's true safe range by nudging its pulse in small steps from home and watching where the joint mechanically binds or the arm collides with itself. Record the values just inside those limits as your jmin/jmax — the arm's real, safe envelope.

   Move in steps of ~25 microseconds and pause. If a joint buzzes without moving, it is straining against a limit — back off. Your recorded limits should stop well short of any grinding.
4. 2.4
   Define a home() routine that returns every joint to its home pulse, and call it in setup() so the arm always starts from the same known pose. A consistent start makes every later motion repeatable.

   cppCopy

   int home_us[4] = {1500, 1500, 1500, 1700};
   
   void home() {
     for (int i = 0; i < 4; i++) setJoint(i, home_us[i]);
   }

Replace instant jumps with smooth interpolated motion so the arm glides between poses — the trajectory idea from the Arm lesson, which stops the arm flinging objects or shaking the bench.

1. 3.1
   Notice the problem first: command a joint straight from one pulse to a far one and watch the arm snap and wobble. Snapping is hard on the servos, the structure and anything the arm is holding — this is exactly what smoothing fixes.

   Check: A direct large jump makes the arm move abruptly and overshoot or shake.
2. 3.2
   Write a smoothing function that moves a joint from its current pulse to a target over many small steps with a short pause each, so the motion ramps instead of jumping. Track each joint's current pulse so you always know where it is.

   cppCopy

   int cur[4] = {1500, 1500, 1500, 1700};
   
   void moveJoint(int i, int target, int stepMs) {
     target = constrain(target, jmin[i], jmax[i]);
     int dir = target > cur[i] ? 1 : -1;
     while (cur[i] != target) {
       cur[i] += dir * min(5, abs(target - cur[i]));  // 5us per step
       setJoint(i, cur[i]);
       delay(stepMs);                                 // pace the ramp
     }
   }

3. 3.3
   Move to a target pose by smoothing all joints together — step every joint a little each cycle toward its target — so the arm moves as one coordinated motion rather than one joint at a time. Coordinated motion looks and behaves far better.

   cppCopy

   void moveTo(int t[4], int stepMs) {
     bool moving = true;
     while (moving) {
       moving = false;
       for (int i = 0; i < 4; i++) {
         int tgt = constrain(t[i], jmin[i], jmax[i]);
         if (cur[i] != tgt) {
           cur[i] += (tgt > cur[i] ? 1 : -1) * min(5, abs(tgt - cur[i]));
           setJoint(i, cur[i]);
           moving = true;
         }
       }
       delay(stepMs);
     }
   }

   Check: The arm glides smoothly from home to a target pose with all joints arriving together, no snapping.
4. 3.4
   Tune the step size and pause until the motion is smooth but not sluggish — smaller steps and longer pauses are gentler but slower. Test a few poses at the speed you settle on, keeping your hand near the power cut in case a pose is wrong.

   Always test a brand-new pose at your slowest smoothing speed first. A pose with a typo can swing the arm hard; slow motion turns that from a hazard into a harmless nudge you can stop.

Let the arm learn a sequence you demonstrate through the Serial Monitor, so you can build a pick-and-place by teaching rather than by guessing numbers.

1. 4.1
   Add serial commands to jog a selected joint up and down by small amounts, so you can pose the arm by hand-through-keyboard. This 'teach mode' lets you find real poses interactively instead of computing pulses.

   cppCopy

   int sel = 0;   // selected joint
   
   void handleSerial() {
     if (!Serial.available()) return;
     char c = Serial.read();
     if (c >= '0' && c <= '3') sel = c - '0';
     else if (c == '+') moveJoint(sel, cur[sel] + 25, 8);
     else if (c == '-') moveJoint(sel, cur[sel] - 25, 8);
     else if (c == 'p') { for (int i=0;i<4;i++){Serial.print(cur[i]);Serial.print(' ');} Serial.println(); }
   }

2. 4.2
   Jog the arm to a pose you want (say, above a pickup spot), press the print key, and copy the four pulse values from the Serial Monitor into a saved pose in your sketch. Repeat for each pose in your sequence — pickup-approach, pickup, lift, place-approach, place.

   cppCopy

   // Poses captured from teach mode {base, shoulder, elbow, gripper}
   int POSE_HOME[4]    = {1500, 1500, 1500, 1900};  // gripper open
   int POSE_OVER_PICK[4]= {1700, 1400, 1600, 1900};
   int POSE_PICK[4]    = {1700, 1650, 1750, 1900};
   int POSE_PLACE[4]   = {1250, 1650, 1750, 1200};  // gripper closed

3. 4.3
   Add gripper open/close helpers using your captured open and closed pulses, and tune the closed value so it holds a light object without straining — too tight and the servo buzzes and overheats, too loose and the object slips.

   cppCopy

   const int GRIP = 3;
   void gripOpen()  { moveJoint(GRIP, 1900, 6); }
   void gripClose() { moveJoint(GRIP, 1200, 6); }

   Check: The gripper closes firmly on a light object and holds it without the servo buzzing continuously.
4. 4.4
   Chain the saved poses with moveTo into a demonstrated routine and run it slowly end to end (without an object first) to confirm every transition is collision-free. Teaching then replaying is exactly how many real arms are programmed.

   Check: The arm runs your taught sequence of poses smoothly and without colliding with itself or the bench.

Go beyond replaying fixed poses: compute the shoulder and elbow angles to place the gripper at an (x, y) point you name, using the two-link inverse kinematics from the Arm lesson.

1. 5.1
   Model the shoulder and elbow as a two-link planar arm with link lengths L1 and L2 (measure yours). Recall from the lesson that inverse kinematics turns a target (x, y) into the two joint angles — and returns nothing if the point is out of reach.

   Keep the base angle separate: the base rotates the whole plane to face the target, and the shoulder+elbow reach out within that plane. This split keeps the maths to the simple two-link case.
2. 5.2
   Implement the two-link inverse kinematics, returning the shoulder and elbow angles for a target, or a failure flag when the point is beyond L1+L2. Guarding the out-of-reach case stops the arm straining toward an impossible point.

   cppCopy

   #include <math.h>
   const float L1 = 10.5, L2 = 9.0;   // cm, measure your arm
   
   bool ik2(float x, float y, float &shoulder, float &elbow) {
     float r2 = x*x + y*y;
     float c2 = (r2 - L1*L1 - L2*L2) / (2*L1*L2);
     if (c2 < -1 || c2 > 1) return false;      // out of reach
     elbow = acos(c2);                          // elbow-down solution
     shoulder = atan2(y, x) - atan2(L2*sin(elbow), L1 + L2*cos(elbow));
     return true;
   }

3. 5.3
   Map the computed angles (radians) to your servo pulses. Each joint has its own zero-angle offset and direction, so calibrate by commanding a known angle and adjusting the mapping until the arm actually reaches where the maths says. This calibration is the bridge between clean geometry and your real, imperfect arm.

   cppCopy

   // angle (radians) -> pulse for a joint, using measured offset & scale
   int angleToUs(int joint, float radians) {
     float deg = radians * 180.0 / PI;
     // tune OFFSET_DEG[] and US_PER_DEG[] per joint by calibration
     extern float OFFSET_DEG[4], US_PER_DEG[4];
     return (int)(1500 + (deg - OFFSET_DEG[joint]) * US_PER_DEG[joint]);
   }

4. 5.4
   Test IK by naming a few reachable (x, y) points and confirming the gripper arrives near each — measure the error with a ruler and refine your link lengths and offsets until it is close. Then try an out-of-reach point and confirm the arm refuses instead of straining.

   Check: The gripper reaches within a centimetre or two of several commanded points, and an out-of-reach point is safely rejected.

Combine everything into one reliable routine — see or know where the object is, reach it, grasp, lift, move and release — the complete manipulation task.

1. 6.1
   Write the pick-and-place as a clear sequence built from your helpers: home with gripper open, move above the pickup, lower to it, close the gripper, lift, rotate the base to the drop zone, lower, open, and return home. Building it from tested pieces makes the whole thing predictable.

   cppCopy

   void pickAndPlace() {
     home();          gripOpen();
     moveTo(POSE_OVER_PICK, 8);
     moveTo(POSE_PICK, 10);
     gripClose();     delay(300);      // let the grip settle
     moveTo(POSE_OVER_PICK, 8);        // lift straight up
     moveTo(POSE_PLACE, 8);
     gripOpen();      delay(300);
     home();
   }

2. 6.2
   Dry-run the whole routine slowly with no object, hand near the power cut, and watch every transition for a collision or an over-reach. Only once it is clean end to end should you place a real object.

   Check: The full routine runs with no object without any collision or strained joint.
3. 6.3
   Place a light object at the pickup spot and run it. Tune the pickup height and the grip-close timing until it lifts the object reliably — a delay after closing the gripper lets the grasp settle before the lift, which fixes most dropped objects.

   Check: The arm picks the object, carries it to the drop zone, and releases it, repeatably over several runs.
4. 6.4
   Add a start trigger (a serial keypress or a button) so the routine runs on demand rather than the instant the arm powers up, and re-confirm home is safe. An arm that waits for a deliberate go is far safer than one that moves on boot.

   Never have a powerful arm begin a motion automatically at power-on — a brown-out reset would then restart the whole swing unexpectedly. Require an explicit trigger, and keep the power cut in reach whenever it runs.
5. 6.5
   Run the complete pick-and-place several times in a row and note any pose that occasionally fails, refining just that pose's numbers. An arm that completes the task reliably — not once, but repeatedly — is the finished project and a real portfolio piece.

   Check: The arm completes the pick-and-place at least five times in a row without a miss or a collision.

The arm jitters constantly or the Arduino resets when several joints move.

The servo supply is inadequate. Four servos under load can pull well over an amp; power them from a dedicated 5–6 V supply (never the Arduino's 5 V pin) with a common ground, and add a large capacitor across the servo supply to absorb the current spikes that cause the jitter and resets.

A joint cannot reach the range you expected, or hits a hard stop.

The servo horn was attached at the wrong angle. Command the joint to its home pulse, detach the horn, re-seat it so the arm sits neutral at home, and re-derive that joint's jmin/jmax by nudging in from home until just before it binds.

The gripper servo buzzes continuously and gets hot when closed.

The closed pulse is straining the servo against the object or a mechanical limit. Ease the closed value until it holds the object without buzzing; a servo that hums constantly is stalled, drawing high current, and will overheat or fail.

Inverse kinematics moves the arm to the wrong place.

Either the link lengths L1/L2 are mismeasured, or the angle-to-pulse offsets and directions per joint are off. Re-measure the links, then calibrate each joint by commanding a known angle and adjusting its offset and microseconds-per-degree until the arm physically matches the geometry.

The arm drops the object between pickup and placing.

The grasp is too loose or the lift starts before the grip settles. Tighten the closed pulse a little, add a short delay after gripClose() before lifting, and lift straight up (through the over-pickup pose) rather than swinging away, so the object is not knocked loose.

• Mount a phone or a Pi camera above the work surface and bring in the Vision lesson: detect a coloured object, convert its pixel position to an (x, y) on the surface, and feed that into your inverse kinematics for a vision-guided pick.
• Add a fourth link angle so the gripper stays level (or points down) regardless of the arm's reach, giving true end-effector orientation control instead of just position.
• Drive the servos through a PCA9685 and replace the blocking delays with a non-blocking motion scheduler, so the arm can move and respond to input at the same time like the rover does.
• Recreate the arm in a Gazebo simulation from the Simulation lesson, solve the inverse kinematics there first, and only then run the validated motions on the real servos — the professional prototype-in-sim workflow.