Merge pull request #1 from danieljabailey/master

Added support for polar cylindrical coordinates.
yorkhackspace · Jun 10, 2015 · 46dd4ab · 46dd4ab
2 parents 8d99cbb + 7fe1205
commit 46dd4ab
Show file tree

Hide file tree

Showing 3 changed files with 46 additions and 3 deletions.
diff --git a/keywords.txt b/keywords.txt
@@ -17,7 +17,11 @@ openGripper	KEYWORD2
 closeGripper	KEYWORD2
 gotoPoint	KEYWORD2
 goDirectlyTo	KEYWORD2
+gotoPointCylinder	KEYWORD2
+goDirectlyToCylinder	KEYWORD2
 isReachable	KEYWORD2
 getX	KEYWORD2
 getY	KEYWORD2
 getZ	KEYWORD2
+getR	KEYWORD2
+getTheta	KEYWORD2
diff --git a/meArm.cpp b/meArm.cpp
@@ -61,7 +61,8 @@ void meArm::begin(int pinBase, int pinShoulder, int pinElbow, int pinGripper) {
   _elbow.attach(_pinElbow);
   _gripper.attach(_pinGripper);
 
-  goDirectlyTo(0, 100, 50);
+  //goDirectlyTo(0, 100, 50);
+  goDirectlyToCylinder(0, 100, 50);
   openGripper();
 }
 
@@ -92,6 +93,27 @@ void meArm::gotoPoint(float x, float y, float z) {
   delay(50);
 }
 
+//Get x and y from theta and r
+void meArm::polarToCartesian(float theta, float r, float& x, float& y){
+    _r = r;
+    _t = theta;
+    x = r*sin(theta);
+    y = r*cos(theta);
+}
+
+//Same as above but for cylindrical polar coodrinates
+void meArm::gotoPointCylinder(float theta, float r, float z){
+    float x, y;
+    polarToCartesian(theta, r, x, y);
+    gotoPoint(x,y,z);
+}
+
+void meArm::goDirectlyToCylinder(float theta, float r, float z){
+    float x, y;
+    polarToCartesian(theta, r, x, y);
+    goDirectlyTo(x,y,z);
+}
+
 //Check to see if possible
 bool meArm::isReachable(float x, float y, float z) {
   float radBase,radShoulder,radElbow;
@@ -100,13 +122,13 @@ bool meArm::isReachable(float x, float y, float z) {
 
 //Grab something
 void meArm::openGripper() {
-  _gripper.write(90);
+  _gripper.write(angle2pwm(_svoGripper,pi/2));
   delay(300);
 }
 
 //Let go of something
 void meArm::closeGripper() {
-  _gripper.write(120);
+  _gripper.write(angle2pwm(_svoGripper,0));
   delay(300);
 }
 
@@ -121,3 +143,10 @@ float meArm::getZ() {
   return _z;
 }
 
+
+float meArm::getR() {
+  return _r;
+}
+float meArm::getTheta() {
+  return _t;
+}
diff --git a/meArm.h b/meArm.h
@@ -36,6 +36,11 @@ class meArm {
     void gotoPoint(float x, float y, float z);
     //Set servos to reach a certain point directly without caring how we get there 
     void goDirectlyTo(float x, float y, float z);
+
+    //Same as above but for cylindrical polar coodrinates
+    void gotoPointCylinder(float theta, float r, float z);
+    void goDirectlyToCylinder(float theta, float r, float z);
+
     //Grab something
     void openGripper();
     //Let go of something
@@ -46,8 +51,13 @@ class meArm {
     float getX();
     float getY();
     float getZ();
+
+    float getR();
+    float getTheta();
   private:
+    void polarToCartesian(float theta, float r, float& x, float& y);
     float _x, _y, _z;
+    float _r, _t;
     Servo _base, _shoulder, _elbow, _gripper;
     ServoInfo _svoBase, _svoShoulder, _svoElbow, _svoGripper;
     int _pinBase, _pinShoulder, _pinElbow, _pinGripper;