meArm.h

00001 /* meArm library York Hack Space May 2014
00002  * A simple control library for Phenoptix' meArm
00003  * Usage:
00004  *   meArm arm;
00005  *   arm.begin(1, 10, 9, 6);
00006  *   arm.openGripper();
00007  *   arm.gotoPoint(-80, 100, 140);
00008  *   arm.closeGripper();
00009  *   arm.gotoPoint(70, 200, 10);
00010  *   arm.openGripper();
00011  */
00012 #ifndef MEARM_H
00013 #define MEARM_H
00014 
00015 const float pi=3.14159265359;
00016 
00017 struct ServoInfo {
00018     int n_min, n_max;   // PWM 'soft' limits - should be just within range
00019     float gain;         // PWM per radian
00020     float zero;         // Theoretical PWM for zero angle
00021 };
00022 
00023 class meArm {
00024   public:
00025     //Full constructor uses calibration data, or can just give pins
00026     meArm(int sweepMinBase=145, int sweepMaxBase=49, float angleMinBase=-pi/4, float angleMaxBase=pi/4,
00027       int sweepMinShoulder=118, int sweepMaxShoulder=22, float angleMinShoulder=pi/4, float angleMaxShoulder=3*pi/4,
00028       int sweepMinElbow=144, int sweepMaxElbow=36, float angleMinElbow=pi/4, float angleMaxElbow=-pi/4,
00029       int sweepMinGripper=115, int sweepMaxGripper=115, float angleMinGripper=pi/4, float angleMaxGripper=pi/4);
00030     //required before running
00031     void begin();
00032     //Travel smoothly from current point to another point
00033     void gotoPoint(float x, float y, float z);
00034     //Set servos to reach a certain point directly without caring how we get there 
00035     void goDirectlyTo(float x, float y, float z);
00036 
00037     //Same as above but for cylindrical polar coodrinates
00038     void gotoPointCylinder(float theta, float r, float z);
00039     void goDirectlyToCylinder(float theta, float r, float z);
00040 
00041     //Grab something
00042     void openGripper();
00043     //Let go of something
00044     void closeGripper();
00045     //Check to see if possible
00046     bool isReachable(float x, float y, float z);
00047     //Current x, y and z
00048     float getX();
00049     float getY();
00050     float getZ();
00051 
00052     float getR();
00053     float getTheta();
00054   private:
00055     void polarToCartesian(float theta, float r, float& x, float& y);
00056     float _x, _y, _z;
00057     float _r, _t;
00058     ServoInfo _svoBase, _svoShoulder, _svoElbow, _svoGripper;
00059     int _pinBase, _pinShoulder, _pinElbow, _pinGripper;
00060 };
00061 
00062 #endif