   }
 }
 
+/**
+ * Homing works by setting servos to 90 degrees, and later to pull 
+ * or push the shoulder DC so that the angle on the arm reads 90 degrees
+ * 
+ * Then homing is stopped and the encoder is reset to zero.
+ */
+bool homing=false;
 
 void loop() {
   if (accel.available())

  // Format A1000 >> A1220   --> Close grip
  // A2255 >> Open Grip
  // A6090 >> 90 deg wrist A6010 --> A6180
  // A3220 or A4220 Move forward backward shoulder
  // A7150 will keep the elbow at zero encoder angle arm vertical. So AA140 will pull it up
  // A8220 clockwise A9220 counter
  // AA180 -> Elbow is now a degree based encoder.  Not rotational.
  // Format A5000  Reset everything.


// =========== DC Control using the encoder.
int targetpos = 0;
int TORQUE=200;

void setTargetPos(int newtargetpos)
{
  targetpos = newtargetpos;
}

bool updatedc(Adafruit_DCMotor *dcmotor, int currentpos)
{
  if (targetpos != currentpos)
  {
    dcmotor->setSpeed(TORQUE);

    if (targetpos < currentpos)
      dcmotor->run(FORWARD);
    else
      dcmotor->run(BACKWARD);

    return false;

  } else {
    dcmotor->setSpeed(0);
    return true;
  }

}
