Skip to content

Instantly share code, notes, and snippets.

@AlexanderBrevig
Last active May 3, 2016 04:42
Show Gist options
  • Save AlexanderBrevig/3be45503ffa45a1814f0 to your computer and use it in GitHub Desktop.
Save AlexanderBrevig/3be45503ffa45a1814f0 to your computer and use it in GitHub Desktop.
A new Braccio library
/*
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
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment