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:
- 46:4497e945de6b
- Parent:
- 45:0db0fc9f77b1
- Child:
- 47:e01ba47d11cc
--- a/Master.cpp Sun Dec 01 07:23:22 2013 +0000 +++ b/Master.cpp Sun Dec 01 20:13:35 2013 +0000 @@ -28,14 +28,38 @@ //Helper function to Detect If we have landed. //NO Input //Output: 0 = Flight Phase. 1 = Ground Phase. - float th3 = bEnc1.getAngle()*6.283; + float th3 = bEnc1.getAngle(); float a = lg*sin(-th3)+h; - float th1 = mEnc1.getAngle()*6.283; - float th2 = mEnc2.getAngle()*6.283-pi/2; - float b = l1+l2*sin(-th1)+l3*sin(-th2+th1)+ l2*sin(-th1); + 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; } +int SLIP(float initLen){//SlIP Model + //Get Length + float th1 = mEnc1.getAngle(); + float th2 = mEnc2.getAngle()-pi/2; + double y = l1+l2*sin(th1)+l3*sin(th2-th1)+ l2*sin(th1); + double x = l2*cos(th1)+l3*cos(th2-th1)+ l2*cos(th1); + float length = pow(pow(x,2.0)+pow(y,2.0),0.5); + int ks = 0.1; + float mag = (initLen-length)*ks; + //Apply Forward Kinematics + Point output = Point(-mag*x/length,-mag*y/length,0); + Joints dtorque = invKinBody(output); + //Apply Motor Torque Control. + m1.setTorque(dtorque.t1); + m2.setTorque(dtorque.t2); + return 1; +} +int FLIGHT(float initM1, float initM2){ + //Pos Control + m1.setPos(initM1); + m2.setPos(initM2); + //Awesome. + return 1; +} int main() { Point p1(0,-.1,0); Point p2(0,-.6,0); @@ -44,20 +68,34 @@ BezCurve curve(p, 3); curve.startCurve(); m1.setPos(0); - m2.setPos(0); + m2.setPos(0); + kankan.start(); + //Initalize Control Varialbes int Phase = 0;//Flight Phase + int initLen = 0.17; + float initM1 = -0.5; + float initM2 = 0.5; + while(true){ if (Phase == 0){//Flight Phase - //Uses Bezian Curve to Control - m1.setPos(-1); + //Uses PD to Control + //FLIGHT(initM1, initM2); + m1.setPos(-0.5); if(landDetection()){ //Landed going to Phase 1 + //Record initial Length. Phase =1; + float th1 = mEnc1.getAngle(); + float th2 = mEnc2.getAngle()-pi/2; + double y = l1+l2*sin(th1)+l3*sin(th2-th1)+ l2*sin(th1); + double x = l2*cos(th1)+l3*cos(th2-th1)+ l2*cos(th1); + initLen = pow(pow(x,2.0)+pow(y,2.0),0.5); } }else{ //Land Phase - //Uses SLIP Model to Control. - m1.setPos(-0.5); + //Uses SLIP Model to Control + //SLIP(initLen); + m1.setPos(-1); if(!landDetection()){ //Lift Up going to Phase 0 Phase =0;