Diff: src/kangaroo.cpp

Revision:

30:18724a8fb2bf

Child:

38:922f2584bdfd

Child:

43:68faf056ed5c

diff -r 33c5c61f1068 -r 18724a8fb2bf src/kangaroo.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/src/kangaroo.cpp	Sun Nov 24 19:52:28 2013 +0000
@@ -0,0 +1,38 @@
+#include "mbed.h"
+#include "include/kangaroo.hpp"
+
+
+Kangaroo::Kangaroo(Motor &M1, Motor &M2, QEI &e1, QEI &e2):enc1(e1),enc2(e2),m1(M1),m2(M2),led1(LED1),led2(LED2),led3(LED3),led4(LED4){
+
+}
+
+void Kangaroo::zero(){
+    m1.setVel(-.002);//wait for the legs to go to stopping points
+    m2.setVel(-.002);
+    led4=1;
+    wait(1);
+    
+    m1.zero();
+    m2.zero();
+    
+    enc1.reset();
+    enc2.reset();
+    led2=1;
+    while(((enc1.getRevolutions()==0)||(enc2.getRevolutions()==0))){
+        wait(0.1);
+    }
+    
+    led1=1;
+    led3=1;
+    
+}
+
+void Kangaroo::testEncoders(Serial &pc){
+    //pc.printf("hello world\n");
+    
+    //pc.printf("motor1:  %i\n",m1.getPos());
+    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());
+}
\ No newline at end of file