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.
Diff: Master.cpp
- Revision:
- 54:17ea4b3c80de
- Parent:
- 53:978b7fa74080
- Child:
- 55:ff84fbdfd1d1
--- a/Master.cpp Mon Dec 02 05:16:26 2013 +0000 +++ b/Master.cpp Mon Dec 02 06:54:48 2013 +0000 @@ -11,8 +11,8 @@ Motor m2(p16,p19,p20,p22,mEnc1);//knee p19 high is CCW -QEI bEnc1(p27, p28, p29, 500, QEI::X4_ENCODING); //track offset:-2.236814 -QEI bEnc2(p5, p6, p7, 500, QEI::X4_ENCODING); //body offset:1.770973 +QEI bEnc1(p27, p28, p29, 2000, QEI::X4_ENCODING); //track offset:-2.236814 +QEI bEnc2(p5, p6, p7, 2000, QEI::X4_ENCODING); //body offset:1.770973 Kangaroo kankan(m1,m2,bEnc1,bEnc2); @@ -24,16 +24,17 @@ PwmOut pwmOut(p21); -int landDetection(){ +int landDetection(Kangaroo &k){ //Helper function to Detect If we have landed. //NO Input //Output: 0 = Flight Phase. 1 = Ground Phase. - float th3 = bEnc1.getAngle(); - float a = lg*sin(-th3)+h; + float th3 = k.getAngle1(); + float a = lg*sin(th3)+h; float th1 = mEnc1.getAngle(); float th2 = mEnc2.getAngle()-pi/2; - float b = l1+l2*sin(th1)+l3*sin(th2-th1)+ l2*sin(th1); - return a<b; + float th4 = k.getAngle2(); + float b = (l1+(l2+l4)*sin(th1)+l3*sin(th2+th1))*sin(th4); + return a<(b+.005); } int SLIP(float initLen){//SlIP Model @@ -65,12 +66,15 @@ return 1; } int main() { - kankan.start(); - kankan.zero(); + //kankan.start(); + //kankan.zero(); while(1){ kankan.testEncoders(pc); wait(0.01); + if(landDetection(kankan)){ + printf("hello ground!\n"); + } } /* Point p1(0,-.1,0); @@ -98,7 +102,7 @@ //m1.setPos(-0.5); led1=0; kankan.setPoint(Point(0,-.08,0)); - if(landDetection()){ + if(landDetection(kankan)){ //Landed going to Phase 1 //Record initial Length. Phase =1; @@ -114,7 +118,7 @@ SLIP(initLen); //m1.setPos(-1); - if(!landDetection()){ + if(!landDetection(kankan)){ //Lift Up going to Phase 0 Phase =0; }