Diff: src/kangaroo.cpp

Revision:

49:3aaa790800ad

Parent:

48:8f0e007bd305

Child:

50:510ce69c2a10

--- a/src/kangaroo.cpp	Sun Dec 01 22:19:48 2013 +0000
+++ b/src/kangaroo.cpp	Sun Dec 01 23:43:58 2013 +0000
@@ -21,13 +21,13 @@
     
     m1.zero();
     m2.zero();
-    
-    m1.setPos(0); //-3.1415/2);
-    m2.setPos(-3.1415/2); //bring legs to neutral position
+    m1.calibAngle(0);
+    m2.calibAngle(2*3.1415/3);
     
+    wait(1);
     
-    m1.zero();  //rezero at neutral
-    m2.zero();
+    m1.setPos(0);
+    m2.setPos(3.1415/2);
     
     enc1.reset();
     enc2.reset();
@@ -38,7 +38,7 @@
     
     led1=1;
     
-    //setPoint(Point(0,-.1,0));
+    //setPoint(Point(0,-.08,0));
     wait(3);
     led3=0;
     led1=0;
@@ -50,15 +50,20 @@
 
 void Kangaroo::testEncoders(Serial &pc){
     //pc.printf("hello world\n");
-    
+    pc.printf("x: %f   y: %f\n", getPoint().x, getPoint().y);
     //pc.printf("motor1:  %i\n",m1.getPos());
-    pc.printf("clix1:  %i, rots1: %i\n",enc1.getPulses(), enc1.getRevolutions());
+    //pc.printf("clix1:  %i, rots1: %i\n",enc1.getPulses(), enc1.getRevolutions());
     
     //pc.printf("motor2:  %i\n",);
-    pc.printf("clix2:  %i, rots2: %i\n",enc2.getPulses(), enc2.getRevolutions());
+    //pc.printf("clix2:  %i, rots2: %i\n",enc2.getPulses(), enc2.getRevolutions());
 }
 
-Point Kangaroo
+Point Kangaroo::getPoint(){
+    float x = (l2+l4)*cos(m1.getAngle())-(l3)*cos(m1.getAngle()+m2.getAngle());
+    float y = (l2+l4)*sin(m1.getAngle())+(l3)*sin(m1.getAngle()+m2.getAngle());
+    
+    return Point(x,y,0);
+}
 
 float Kangaroo::getAngle(){
     return (enc2.getAngle())+1.770973;