Carlo Collodi / kangaroo

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   QEI mbed

src/kangaroo.cpp@55:ff84fbdfd1d1, 2013-12-02 (annotated)

Committer:
calamaridudeman
Date:
Mon Dec 02 23:15:15 2013 +0000
Revision:
55:ff84fbdfd1d1
Parent:
54:17ea4b3c80de
Child:
56:7015e2e79ea7
everything except SLIP/flight stuff works

Who changed what in which revision?

UserRevisionLine numberNew contents of line
calamaridudeman 37:bf257a0154db 1 #include "mbed.h"
calamaridudeman 37:bf257a0154db 2 #include "include/kangaroo.hpp"
calamaridudeman 37:bf257a0154db 3
calamaridudeman 37:bf257a0154db 4
calamaridudeman 43:68faf056ed5c 5
calamaridudeman 37:bf257a0154db 6 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){
calamaridudeman 37:bf257a0154db 7
calamaridudeman 55:ff84fbdfd1d1 8 joint_h=0;
calamaridudeman 55:ff84fbdfd1d1 9 leg_y = 0;
calamaridudeman 55:ff84fbdfd1d1 10
calamaridudeman 37:bf257a0154db 11 }
calamaridudeman 37:bf257a0154db 12
calamaridudeman 37:bf257a0154db 13 void Kangaroo::zero(){
calamaridudeman 43:68faf056ed5c 14 m1.start();
calamaridudeman 43:68faf056ed5c 15 m2.start();
calamaridudeman 43:68faf056ed5c 16
calamaridudeman 55:ff84fbdfd1d1 17 m1.setVel(.5);//use in final
calamaridudeman 55:ff84fbdfd1d1 18 m2.setVel(.3);
calamaridudeman 53:978b7fa74080 19 //m2.setPos(3.1415/5);//bring legs to hard stop
calamaridudeman 43:68faf056ed5c 20
calamaridudeman 37:bf257a0154db 21 led4=1;
calamaridudeman 53:978b7fa74080 22 wait(5.0);
calamaridudeman 37:bf257a0154db 23
sherryxy 50:510ce69c2a10 24 //m1.zero();
sherryxy 50:510ce69c2a10 25 //m2.zero();
calamaridudeman 53:978b7fa74080 26 m1.calibAngle(0.0);
calamaridudeman 53:978b7fa74080 27 m2.calibAngle(2.44);
calamaridudeman 43:68faf056ed5c 28
calamaridudeman 55:ff84fbdfd1d1 29 wait(.1);
calamaridudeman 55:ff84fbdfd1d1 30
sherryxy 48:8f0e007bd305 31
calamaridudeman 55:ff84fbdfd1d1 32 m1.setPos(-3.1415/12);
calamaridudeman 55:ff84fbdfd1d1 33 m2.setPos(3.1415/2);
calamaridudeman 43:68faf056ed5c 34
calamaridudeman 37:bf257a0154db 35 enc1.reset();
calamaridudeman 37:bf257a0154db 36 enc2.reset();
calamaridudeman 37:bf257a0154db 37 led2=1;
calamaridudeman 55:ff84fbdfd1d1 38 while(((enc1.getRevolutions()==0)||(enc2.getRevolutions()==0))){
calamaridudeman 37:bf257a0154db 39 wait(0.1);
calamaridudeman 55:ff84fbdfd1d1 40 }
calamaridudeman 37:bf257a0154db 41
calamaridudeman 55:ff84fbdfd1d1 42 landDetection();
calamaridudeman 37:bf257a0154db 43 led1=1;
calamaridudeman 43:68faf056ed5c 44
sherryxy 49:3aaa790800ad 45 //setPoint(Point(0,-.08,0));
calamaridudeman 55:ff84fbdfd1d1 46 wait(3);
sherryxy 48:8f0e007bd305 47 led3=0;
sherryxy 48:8f0e007bd305 48 led1=0;
sherryxy 48:8f0e007bd305 49 led2=0;
sherryxy 48:8f0e007bd305 50 led4=0;
sherryxy 48:8f0e007bd305 51 //m1.stop();
sherryxy 48:8f0e007bd305 52 //m2.stop();
calamaridudeman 37:bf257a0154db 53 }
calamaridudeman 37:bf257a0154db 54
calamaridudeman 55:ff84fbdfd1d1 55 float Kangaroo::jointHeight(){
calamaridudeman 55:ff84fbdfd1d1 56 return joint_h;
calamaridudeman 55:ff84fbdfd1d1 57 }
calamaridudeman 55:ff84fbdfd1d1 58
calamaridudeman 55:ff84fbdfd1d1 59 float Kangaroo::legY(){
calamaridudeman 55:ff84fbdfd1d1 60 return leg_y;
calamaridudeman 55:ff84fbdfd1d1 61 }
calamaridudeman 55:ff84fbdfd1d1 62
calamaridudeman 37:bf257a0154db 63 void Kangaroo::testEncoders(Serial &pc){
calamaridudeman 37:bf257a0154db 64 //pc.printf("hello world\n");
sherryxy 50:510ce69c2a10 65 //pc.printf("x: %f y: %f\n", getPoint().x, getPoint().y);
calamaridudeman 55:ff84fbdfd1d1 66 pc.printf("me: %f joint: %f length: %f\n",getAngle2(), jointHeight(), legY());
sherryxy 49:3aaa790800ad 67 //pc.printf("clix1: %i, rots1: %i\n",enc1.getPulses(), enc1.getRevolutions());
calamaridudeman 37:bf257a0154db 68
calamaridudeman 37:bf257a0154db 69 //pc.printf("motor2: %i\n",);
sherryxy 49:3aaa790800ad 70 //pc.printf("clix2: %i, rots2: %i\n",enc2.getPulses(), enc2.getRevolutions());
calamaridudeman 43:68faf056ed5c 71 }
calamaridudeman 43:68faf056ed5c 72
calamaridudeman 55:ff84fbdfd1d1 73 int Kangaroo::landDetection(){
calamaridudeman 55:ff84fbdfd1d1 74 //Helper function to Detect If we have landed.
calamaridudeman 55:ff84fbdfd1d1 75 //NO Input
calamaridudeman 55:ff84fbdfd1d1 76 //Output: 0 = Flight Phase. 1 = Ground Phase.
calamaridudeman 55:ff84fbdfd1d1 77 float th3 = getAngle1();
calamaridudeman 55:ff84fbdfd1d1 78 joint_h = lg*sin(th3)+h;
calamaridudeman 55:ff84fbdfd1d1 79 float th4 = getAngle2();
calamaridudeman 55:ff84fbdfd1d1 80 leg_y = -1*(-l1+getPoint().y)*cos(th4);
calamaridudeman 55:ff84fbdfd1d1 81 return joint_h<(leg_y+.013);
calamaridudeman 55:ff84fbdfd1d1 82 }
calamaridudeman 55:ff84fbdfd1d1 83
sherryxy 49:3aaa790800ad 84 Point Kangaroo::getPoint(){
calamaridudeman 55:ff84fbdfd1d1 85 float x = (l2+l4)*cos(m1.getAngle())+(l3)*cos(m1.getAngle()+m2.getAngle()-3.1415);
calamaridudeman 55:ff84fbdfd1d1 86 float y = (l2+l4)*sin(m1.getAngle())+(l3)*sin(m1.getAngle()+m2.getAngle()-3.1415);
sherryxy 49:3aaa790800ad 87
sherryxy 49:3aaa790800ad 88 return Point(x,y,0);
sherryxy 49:3aaa790800ad 89 }
sherryxy 48:8f0e007bd305 90
calamaridudeman 54:17ea4b3c80de 91 float Kangaroo::getAngle2(){
calamaridudeman 55:ff84fbdfd1d1 92 return (enc2.getAngle())-1.65;
calamaridudeman 54:17ea4b3c80de 93 }
calamaridudeman 54:17ea4b3c80de 94
calamaridudeman 54:17ea4b3c80de 95
calamaridudeman 54:17ea4b3c80de 96 float Kangaroo::getAngle1(){
calamaridudeman 54:17ea4b3c80de 97 return -(enc1.getAngle()-0.5372);
calamaridudeman 43:68faf056ed5c 98 }
calamaridudeman 43:68faf056ed5c 99
calamaridudeman 43:68faf056ed5c 100 void Kangaroo::setPoint(Point p){
calamaridudeman 43:68faf056ed5c 101
calamaridudeman 43:68faf056ed5c 102 Joints motors = invKinBody(p);
calamaridudeman 43:68faf056ed5c 103 m1.setPos(motors.t1);
calamaridudeman 43:68faf056ed5c 104 m2.setPos(motors.t2);
calamaridudeman 43:68faf056ed5c 105
calamaridudeman 43:68faf056ed5c 106 }
calamaridudeman 43:68faf056ed5c 107
calamaridudeman 43:68faf056ed5c 108 void Kangaroo::flightPath(){
calamaridudeman 43:68faf056ed5c 109
calamaridudeman 43:68faf056ed5c 110 }
calamaridudeman 43:68faf056ed5c 111
calamaridudeman 43:68faf056ed5c 112 void Kangaroo::start(){
calamaridudeman 43:68faf056ed5c 113 m1.start();
calamaridudeman 43:68faf056ed5c 114 m2.start();
calamaridudeman 43:68faf056ed5c 115 }