meArm.cpp

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  
00013 //Convert from radians to degrees.
00014 #define toDegrees(x) (x * 57.2957795)
00015 //Convert from degrees to radians.
00016 #define toRadians(x) (x * 0.01745329252)
00017 
00018 #include "mbed.h"
00019 #include "ik.h"
00020 #include "meArm.h"
00021 #include "Servo.h"
00022 
00023 Servo _base(p21);
00024 Servo _shoulder(p22);
00025 Servo _elbow(p23);
00026 Servo _gripper(p24);
00027 
00028 bool setup_servo (ServoInfo& svo, const int n_min, const int n_max,
00029                   const float a_min, const float a_max)
00030 {
00031     float n_range = n_max - n_min;
00032     float a_range = a_max - a_min;
00033 
00034     // Must have a non-zero angle range
00035     if(a_range == 0) return false;
00036 
00037     // Calculate gain and zero
00038     svo.gain = n_range / a_range;
00039     svo.zero = n_min - svo.gain * a_min;
00040 
00041     // Set limits
00042     svo.n_min = n_min;
00043     svo.n_max = n_max;
00044 
00045     return true;
00046 }
00047 
00048 int angle2pwm (const ServoInfo& svo, const float angle)
00049 {
00050     float pwm = 0.5f + svo.zero + svo.gain * angle;
00051     return int(pwm);
00052 }
00053 
00054 //Full constructor with calibration data
00055 meArm::meArm(int sweepMinBase, int sweepMaxBase, float angleMinBase, float angleMaxBase,
00056              int sweepMinShoulder, int sweepMaxShoulder, float angleMinShoulder, float angleMaxShoulder,
00057              int sweepMinElbow, int sweepMaxElbow, float angleMinElbow, float angleMaxElbow,
00058              int sweepMinGripper, int sweepMaxGripper, float angleMinGripper, float angleMaxGripper)
00059 {
00060     setup_servo(_svoBase, sweepMinBase, sweepMaxBase, angleMinBase, angleMaxBase);
00061     setup_servo(_svoShoulder, sweepMinShoulder, sweepMaxShoulder, angleMinShoulder, angleMaxShoulder);
00062     setup_servo(_svoElbow, sweepMinElbow, sweepMaxElbow, angleMinElbow, angleMaxElbow);
00063     setup_servo(_svoGripper, sweepMinGripper, sweepMaxGripper, angleMinGripper, angleMaxGripper);
00064 }
00065 
00066 void meArm::begin() {  
00067   printf("Begun!");
00068   goDirectlyTo(0, 100, 50);
00069 }
00070 
00071 
00072 //Set servos to reach a certain point directly without caring how we get there 
00073 void meArm::goDirectlyTo(float x, float y, float z) {
00074   float radBase,radShoulder,radElbow;
00075   if (solve(x, y, z, radBase, radShoulder, radElbow)) {
00076     _base.position(angle2pwm(_svoBase,toDegrees(radBase))/100);
00077     _shoulder.position(angle2pwm(_svoShoulder,toDegrees(radShoulder))/100);
00078     _elbow.position(angle2pwm(_svoElbow,toDegrees(radElbow))/100);
00079     _x = x; _y = y; _z = z;
00080   }    
00081 }
00082 
00083 //Travel smoothly from current point to another point
00084 void meArm::gotoPoint(float x, float y, float z) {
00085   //Starting points - current pos
00086   float x0 = _x; 
00087   float y0 = _y; 
00088   float z0 = _z;
00089   float dist = sqrt((x0-x)*(x0-x)+(y0-y)*(y0-y)+(z0-z)*(z0-z));
00090   int step = 5;
00091   for (int i = 0; i<dist; i+= step) {
00092     goDirectlyTo(x0 + (x-x0)*i/dist, y0 + (y-y0) * i/dist, z0 + (z-z0) * i/dist);
00093   }
00094   goDirectlyTo(x, y, z);
00095   wait(1);
00096 }
00097 
00098 //Get x and y from theta and r
00099 void meArm::polarToCartesian(float theta, float r, float& x, float& y){
00100     _r = r;
00101     _t = theta;
00102     x = r*sin(theta);
00103     y = r*cos(theta);
00104 }
00105 
00106 //Same as above but for cylindrical polar coodrinates
00107 void meArm::gotoPointCylinder(float theta, float r, float z){
00108     float x, y;
00109     polarToCartesian(theta, r, x, y);
00110     gotoPoint(x,y,z);
00111 }
00112 
00113 void meArm::goDirectlyToCylinder(float theta, float r, float z){
00114     float x, y;
00115     polarToCartesian(theta, r, x, y);
00116     goDirectlyTo(x,y,z);
00117 }
00118 
00119 //Check to see if possible
00120 bool meArm::isReachable(float x, float y, float z) {
00121   float radBase,radShoulder,radElbow;
00122   return (solve(x, y, z, radBase, radShoulder, radElbow));
00123 }
00124 
00125 //Grab something
00126 void meArm::openGripper() {
00127   _gripper.write(1.0);
00128   wait(0.5);
00129 }
00130 
00131 //Let go of something
00132 void meArm::closeGripper() {
00133   _gripper.write(0.0);
00134   wait(0.5);
00135 }
00136 
00137 //Current x, y and z
00138 float meArm::getX() {
00139   return _x;
00140 }
00141 float meArm::getY() {
00142   return _y;
00143 }
00144 float meArm::getZ() {
00145   return _z;
00146 }
00147 
00148 
00149 float meArm::getR() {
00150   return _r;
00151 }
00152 float meArm::getTheta() {
00153   return _t;
00154 }