Spin Pop Quiz, what's wrong with the SetPosition method.

Martin_H

The SetPosition method in the code below contains the same bug in two places. Can you spot it? I felt pretty silly when I finally did.

I'll check back later and see if anyone got it. I'll also post the corrected code in case no one does.

CON
         _clkmode       = xtal1 + pll16x
        _xinfreq        = 5_000_000

        ' Angular values for common angles
        RIGHT_ANGLE     = pi / 2.0
        STRAIGHT_ANGLE  = pi
        FULL_ROTATION   = 2.0 * pi

        ' Arm dimensions in mm
        BASE_HGT        = 95    ' base hight 3.75"
        HUMERUS         = 133   ' shoulder-to-elbow "bone" 5.25"
        ULNA            = 157   ' elbow-to-wrist "bone" 6 3/16"
        GRIPPER         = 127   ' gripper (incl. wrist rotate mechanism) length 5"

        ' Servo names and pin assignments.
        BASE_SERVO      = 0
        SHOL_SERVO      = 1
        ELBO_SERVO      = 2
        WRIS_SERVO      = 4
        WROT_SERVO      = 5
        GRIP_SERVO      = 6

        ' pre-calculations for right triangle trig.
        HUME_SQ = HUMERUS * HUMERUS
        ULNA_SQ = ULNA * ULNA
 
OBJ
        f32     : "F32"
        fs      : "FloatString"
        pst     : "Parallax Serial Terminal"
        servo   : "Servo32v7.spin" 

VAR
        ' Coordinates in arm polar space.
        long m_baseAngleRads
        long m_sholAngleRads
        long m_elboAngleRads
        long m_wrisAngleRads
        long m_wrisRotAngleRads

        ' Coordinate of gripper tip in cartesian space.
        long m_x
        long m_y
        long m_z
        long m_wrisGroundAngleRads

' Start and Stop methods removed for brevity as they aren't the problem.
     
PUB SetPosition(x, y, z, wristAngle) | grip_off_y, grip_off_z, wrist_y, wrist_z, s_w, s_w_sqrt, a1, a2, elb_angle_rn
{{
  SetPosition : Arm positioning routine utilizing inverse kinematics.  Since the arm is
    resting on a surface Z can only be positive.  Servo movement contraints prevent y
    from being negative, But X can be a signed value.
  Parameters:
    x   integer side to side displacement.
    y   integer distance out from the base center.
    z   integer height above the table
    wristAngle    wrist angle of the gripper in radians.
}}
  ' Save the cartesian space coordinates.
  m_x := x
  m_y := y
  m_z := z
  m_wrisGroundAngleRads := wristAngle

  ' Convert x and y to float and compute a polar angle to position the base.
  SetBase(f32.ATan2(f32.FFloat(x), f32.FFloat(y)))

  ' Compute the vector magnitude and hold in the y coordinate.
  y := ^^((x * x) + (y * y))

  ' Convert gripper length to float and use for Gripper offsets calculated
  ' based on grip angle and convert back to integer with rounding.
  grip_off_z := f32.FRound(f32.FMul(f32.Sin(wristAngle), FLOAT(GRIPPER)))
  grip_off_y := f32.FRound(f32.FMul(f32.Cos(wristAngle), FLOAT(GRIPPER)))

  ' Wrist position
  wrist_z := (z - grip_off_z) - BASE_HGT
  wrist_y := y - grip_off_y

  ' Shoulder to wrist distance is a hypotenuse ( AKA sw )
  s_w := (wrist_z * wrist_z) + (wrist_y * wrist_y)
  s_w_sqrt := ^^ s_w

  ' s_w angle to ground
  a1 := f32.ATan2(f32.FFloat(wrist_z), f32.FFloat(wrist_y))

  ' s_w angle to humerus
  a2 := f32.ACos(f32.FFloat( ((HUME_SQ - ULNA_SQ) + s_w) / (2 * HUMERUS * s_w_sqrt) ))

  ' Position servo to shoulder angle and set the shoulder data member
  SetShoulder(f32.FAdd(a1, a2))

  ' Position servo to elbow angle and set the elbow data member.
  SetElbow(f32.ACos(f32.FFloat((HUME_SQ + ULNA_SQ - s_w) / (2 * HUMERUS * ULNA))))
  elb_angle_rn := f32.FNeg(f32.FSub(STRAIGHT_ANGLE, m_elboAngleRads))

  ' wrist angle
  SetWristAngle(f32.FSub(f32.FSub(wristAngle, elb_angle_rn), m_sholAngleRads))

' Set methods removed for brevity as they aren't the problem.

Martin_H

It's probably to big a bite of code for a code puzzler. Look at the arc cosine call.

Martin_H

Spin's math operators are all integer. However, even when you've made sure of your data types and converted to float, you can still have bugs. This is the corrected ACos call:

' s_w angle to humerus
a2 := f32.ACos( f32.FDiv( f32.FFloat((HUME_SQ - ULNA_SQ) + s_w), f32.FFloat(2 * HUMERUS * s_w_sqrt) ) )

The second one needed the same fix. A real groaner when I found it.