Diff: src/kangaroo.cpp

Revision:

54:17ea4b3c80de

Parent:

53:978b7fa74080

Child:

55:ff84fbdfd1d1

diff -r 978b7fa74080 -r 17ea4b3c80de src/kangaroo.cpp
--- a/src/kangaroo.cpp	Mon Dec 02 05:16:26 2013 +0000
+++ b/src/kangaroo.cpp	Mon Dec 02 06:54:48 2013 +0000
@@ -26,7 +26,7 @@
     wait(1);
     
     //m1.setPos(-3.1415/2);
-    m2.setPos(3.1415/2);
+    //m2.setPos(3.1415/2);
     
     enc1.reset();
     enc2.reset();
@@ -35,6 +35,7 @@
         wait(0.1);
     }*/
     
+    
     led1=1;
     
     //setPoint(Point(0,-.08,0));
@@ -50,7 +51,7 @@
 void Kangaroo::testEncoders(Serial &pc){
     //pc.printf("hello world\n");
     //pc.printf("x: %f   y: %f\n", getPoint().x, getPoint().y);
-    pc.printf("desir:  %f  motor2:  %i\n",m2.dAngle, m2.getPos());
+    pc.printf("me:  %f  beam:  %f\n",getAngle2(), getAngle1());
     //pc.printf("clix1:  %i, rots1: %i\n",enc1.getPulses(), enc1.getRevolutions());
     
     //pc.printf("motor2:  %i\n",);
@@ -64,8 +65,13 @@
     return Point(x,y,0);
 }
 
-float Kangaroo::getAngle(){
-    return (enc2.getAngle())+1.770973;
+float Kangaroo::getAngle2(){
+    return (enc2.getAngle())-1.56;
+}
+
+
+float Kangaroo::getAngle1(){
+    return -(enc1.getAngle()-0.5372);
 }
 
 void Kangaroo::setPoint(Point p){