Diff: src/kangaroo.cpp

Revision:

41:65de628f701f

Parent:

38:922f2584bdfd

--- a/src/kangaroo.cpp	Tue Nov 26 04:42:39 2013 +0000
+++ b/src/kangaroo.cpp	Tue Nov 26 19:29:34 2013 +0000
@@ -2,6 +2,7 @@
 #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){
 
 }
@@ -12,8 +13,8 @@
     //m1.setVel(-.2);//wait for the legs to go to stopping points
     //m2.setVel(-.2);
     
-    m1.setPos(2.0);
-    m2.setPos(2.0);
+    m1.setPos(3.1415/6);
+    m2.setPos(3.1415/6);//bring legs to hard stop
     
     led4=1;
     wait(3.0);
@@ -21,6 +22,12 @@
     m1.zero();
     m2.zero();
     
+    m1.setPos(-3.1415/2);
+    m2.setPos(-3.1415/2); //bring legs to neutral position
+    
+    m1.zero();  //rezero at neutral
+    m2.zero();
+    
     enc1.reset();
     enc2.reset();
     led2=1;
@@ -29,6 +36,9 @@
     }
     
     led1=1;
+    
+    setPoint(Point(1.0,-1.0,0));
+    
     led3=1;
     m1.stop();
     m2.stop();
@@ -46,4 +56,23 @@
 
 float Kangaroo::getAngle(){
     return (enc2.getAngle())+1.770973;
-}
\ No newline at end of file
+}
+
+void Kangaroo::setPoint(Point p){
+
+    Joints motors = invKinBody(p);
+    m1.setPos(motors.t1);
+    m2.setPos(motors.t2);
+
+}
+
+void Kangaroo::flightPath(){
+
+}
+
+void Kangaroo::start(){
+    m1.setPos(0.0);
+    m2.setPos(0.0);
+    m1.start();
+    m2.start();
+}