Carlo Collodi / kangaroo

Dependencies:   QEI mbed

Revision:
50:510ce69c2a10
Parent:
49:3aaa790800ad
Child:
53:978b7fa74080
--- a/src/kangaroo.cpp	Sun Dec 01 23:43:58 2013 +0000
+++ b/src/kangaroo.cpp	Sun Dec 01 23:56:18 2013 +0000
@@ -17,29 +17,29 @@
     m2.setPos(3.1415/5);//bring legs to hard stop
     
     led4=1;
-    wait(3.0);
+    wait(1.0);
     
-    m1.zero();
-    m2.zero();
+    //m1.zero();
+    //m2.zero();
     m1.calibAngle(0);
     m2.calibAngle(2*3.1415/3);
     
     wait(1);
     
-    m1.setPos(0);
-    m2.setPos(3.1415/2);
+    m1.setPos(-3.1415/2);
+    //m2.setPos(3.1415/2);
     
     enc1.reset();
     enc2.reset();
     led2=1;
-    while(((enc1.getRevolutions()==0)||(enc2.getRevolutions()==0))){
+    /*while(((enc1.getRevolutions()==0)||(enc2.getRevolutions()==0))){
         wait(0.1);
-    }
+    }*/
     
     led1=1;
     
     //setPoint(Point(0,-.08,0));
-    wait(3);
+    //wait(3);
     led3=0;
     led1=0;
     led2=0;
@@ -50,8 +50,8 @@
 
 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("x: %f   y: %f\n", getPoint().x, getPoint().y);
+    pc.printf("motor1:  %i  motor2:  %i\n",m1.getPos(), m2.getPos());
     //pc.printf("clix1:  %i, rots1: %i\n",enc1.getPulses(), enc1.getRevolutions());
     
     //pc.printf("motor2:  %i\n",);