A new Braccio library

Braccio.h

/*
  Braccio.h - Library for controlling the Arduino Robotic Arm; Braccio!
  Copyright (c) 2016 - Alexander Brevig & Brett Hagman
  
  This library is free software; you can redistribute it and/or
  modify it under the terms of the GNU Lesser General Public
  License as published by the Free Software Foundation; either
  version 2.1 of the License, or (at your option) any later version.
  This library is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  Lesser General Public License for more details.
  You should have received a copy of the GNU Lesser General Public
  License along with this library; if not, write to the Free Software
  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
*/

#ifndef BRACCIO_H
#define BRACCIO_H

#include <Arduino.h>
#include <Servo.h>

class Braccio2
{
    typedef uint32_t btime_t;
  public:
    Braccio2(Servo &_base, Servo &_shoulder, Servo &_elbow, Servo &_wristRot, Servo &_wristVer, Servo &_gripper)
    {
      servos[BASE] = &_base;
      servos[SHOULDER] = &_shoulder;
      servos[ELBOW] = &_elbow;
      servos[WRIST_ROTATE] = &_wristRot;
      servos[WRIST_VERTICAL] = &_wristVer;
      servos[GRIPPER] = &_gripper;
      lastTime = millis();
      for (int i = 0; i < SERVOS; i++)
      {
        currentPositions[i] = 90;
        targetPositions[i] = 90;
      }
    }

    void home(btime_t timeSpentMoving = 0)
    {
      targetPositions[BASE] = 90;
      targetPositions[SHOULDER] = 45;
      targetPositions[ELBOW] = 180;
      targetPositions[WRIST_ROTATE] = 180;
      targetPositions[WRIST_VERTICAL] = 90;
      targetPositions[GRIPPER] = 10;
      moveTime = timeSpentMoving;

      while (update()); //make sure we get there
    }

    //TODO decide on a name for polling
    bool update()
    {
      // let's just figure out our progress percentage based on the moveTime
      float progress = (float)map(millis(), lastTime, lastTime + moveTime, 0, 1000) / 1000;
      bool processing = false;

      for (int i = 0; i < SERVOS; i++)
      {
        // Move the servo to the new percentage
        uint16_t currentStep = ((float)targetPositions[i] - currentPositions[i]) * progress;
        servos[i]->write(currentPositions[i] + currentStep);
        if (progress < 1)
        {
          processing = true;
        }
        else
        {
          currentPositions[i] = targetPositions[i];
        }
      }
      return processing;
    }

    void move(btime_t timeSpentMoving = 0)
    {
      moveTime = timeSpentMoving;
      lastTime = millis();
      while (update()); //go there now!
    }

    void base(Servo &base) { servos[BASE] = &base; }
    void base(uint8_t target) { targetPositions[BASE] = target; }

    void shoulder(Servo &shoulder) { servos[SHOULDER] = &shoulder; }
    void shoulder(uint8_t target) { targetPositions[SHOULDER] = target; }

    void elbow(Servo &elbow) { servos[ELBOW] = &elbow; }
    void elbow(uint8_t target) { targetPositions[ELBOW] = target; }

    void wristRotate(Servo &wristRotate) { servos[WRIST_ROTATE] = &wristRotate; }
    void wristRotate(uint8_t target) { targetPositions[WRIST_ROTATE] = target; }

    //TODO RENAME THIS
    void wristVertical(Servo &wristVertical) { servos[WRIST_VERTICAL] = &wristVertical; }
    void wristVertical(uint8_t target) { targetPositions[WRIST_VERTICAL] = target; }

    void gripper(Servo &gripper) { servos[GRIPPER] = &gripper; }
    void gripper(uint8_t target) { targetPositions[GRIPPER] = target; }

    /// NOT SURE WE SHOULD ALLOW ACCESS THROUGH HERE...?
    //Servo &base(){ return safe(BASE); }
    //Servo &shoulder(){ return safe(SHOULDER); }
    //Servo &elbow(){ return safe(ELBOW); }
    //Servo &wristRotate(){ return safe(WRIST_ROTATE); }
    //Servo &wristVertical(){ return safe(WRIST_VERTICAL); }
    //Servo &gripper(){ return safe(GRIPPER); }

  private:
    Servo &safe(uint8_t index)
    {
      if (servos[index])
      {
        return *servos[index];
      }
      else
      {
        return none;
      }
    }
    static const uint8_t BASE = 0;
    static const uint8_t SHOULDER = 1;
    static const uint8_t ELBOW = 2;
    static const uint8_t WRIST_ROTATE = 3;
    static const uint8_t WRIST_VERTICAL = 4;
    static const uint8_t GRIPPER = 5;
    static const uint8_t SERVOS = 6;
    static Servo none;
    Servo *servos[6];
    uint16_t targetPositions[6];
    uint16_t currentPositions[6];
    btime_t lastTime; //todo dataype
    btime_t moveTime;
};

Servo Braccio2::none = Servo();

#endif

AS A SUMMARY OF OUR DISCUSSION AND A NOTE TO SELF:

I think we can remove static Servo none; and the safe(index):Servo method.

The library should allocate the six servos with Servo servos[]; and then expose attachXXX(pin) for all six motors. This would also make room for useShield() which would fill in the standard for the shield.

Make an asynchronous version of move(time) that relies on the user calling update() in regular (not fixed) intervals. The interpolation currently handles any time span between calls.