A new Braccio library

Braccio.h

/*
  Braccio.h - Library for controlling the Arduino Robotic Arm; Braccio!
  Copyright (c) 2016 - Alexander Brevig 
  
  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>

class Braccio {
    typedef btime_t uint32_t;
public:
    Braccio(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] = 0;
        }
    }

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

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

    //TODO decide on a name for polling
    bool update() {
        btime_t dt = millis() - lastTime; //find delta time
        float progress = 1;
        if (moveTime > 0){
            progress = (float)dt / moveTime;
        }
        if (progress>1){ progress = 1; } //constrain progress to never be more than 1
        bool done = true;
        for (int i=0; i<SERVOS; i++){
            // progress each servo from its start toward its goal
            // by the progress factor based on delta time (makes it update frequency independent)
            uint16_t currentStep = min((float)targetPositions[i] - currentPositions[i]) * progress; 
            servos[i]->write(currentStep);
            if (progress < 1){
                done = false;
            } else {
                currentPositions[i] = targetPositions[i];
            }
        }
        lastTime = millis();
        if (done) {
            moveTime = 0;
        }
        return done;
    }

    void move(btime_t timeSpentMoving = 0){
        moveTime = timeSpentMoving;
        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 Braccio::none = Servo();

#endif