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

Master.cpp@47:e01ba47d11cc, 2013-12-01 (annotated)

Committer:
alexc89
Date:
Sun Dec 01 20:34:31 2013 +0000
Revision:
47:e01ba47d11cc
Parent:
46:4497e945de6b
Child:
48:8f0e007bd305
Pushing again;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
calamaridudeman 23:112c0be5a7f3 1 #include "mbed.h"
calamaridudeman 37:bf257a0154db 2 #include "Master.hpp"
calamaridudeman 22:4d85d989af08 3
calamaridudeman 23:112c0be5a7f3 4 Serial pc(USBTX, USBRX);
calamaridudeman 37:bf257a0154db 5 QEI mEnc1(p25, p26, NC, 1200, QEI::X4_ENCODING); //hip
calamaridudeman 37:bf257a0154db 6 QEI mEnc2(p23, p24, NC, 1200, QEI::X4_ENCODING);//knee
calamaridudeman 37:bf257a0154db 7
calamaridudeman 43:68faf056ed5c 8 Motor m1(p15,p17,p18,p21,mEnc2);//hip p17 high is CCW
calamaridudeman 43:68faf056ed5c 9 Motor m2(p16,p19,p20,p22,mEnc1);//knee p19 high is CCW
calamaridudeman 25:8a34b8d6cc6e 10
alexc89 45:0db0fc9f77b1 11
calamaridudeman 43:68faf056ed5c 12 QEI bEnc1(p27, p28, p29, 500, QEI::X4_ENCODING); //track offset:-2.236814
calamaridudeman 43:68faf056ed5c 13 QEI bEnc2(p5, p6, p7, 500, QEI::X4_ENCODING); //body offset:1.770973
calamaridudeman 37:bf257a0154db 14
calamaridudeman 37:bf257a0154db 15 Kangaroo kankan(m1,m2,bEnc1,bEnc2);
calamaridudeman 37:bf257a0154db 16
calamaridudeman 43:68faf056ed5c 17 Ticker t;
calamaridudeman 43:68faf056ed5c 18
calamaridudeman 43:68faf056ed5c 19 AnalogIn aIn(p15);
alexc89 45:0db0fc9f77b1 20 DigitalOut Forward(p17);
alexc89 45:0db0fc9f77b1 21 DigitalOut Backward(p18);
alexc89 45:0db0fc9f77b1 22 PwmOut pwmOut(p21);
alexc89 45:0db0fc9f77b1 23
alexc89 45:0db0fc9f77b1 24
alexc89 45:0db0fc9f77b1 25 int landDetection(){
alexc89 45:0db0fc9f77b1 26 //Helper function to Detect If we have landed.
alexc89 45:0db0fc9f77b1 27 //NO Input
alexc89 45:0db0fc9f77b1 28 //Output: 0 = Flight Phase. 1 = Ground Phase.
alexc89 46:4497e945de6b 29 float th3 = bEnc1.getAngle();
alexc89 45:0db0fc9f77b1 30 float a = lg*sin(-th3)+h;
alexc89 46:4497e945de6b 31 float th1 = mEnc1.getAngle();
alexc89 46:4497e945de6b 32 float th2 = mEnc2.getAngle()-pi/2;
alexc89 46:4497e945de6b 33 float b = l1+l2*sin(th1)+l3*sin(th2-th1)+ l2*sin(th1);
alexc89 45:0db0fc9f77b1 34 return a<b;
alexc89 45:0db0fc9f77b1 35 }
calamaridudeman 43:68faf056ed5c 36
alexc89 46:4497e945de6b 37 int SLIP(float initLen){//SlIP Model
alexc89 46:4497e945de6b 38 //Get Length
alexc89 46:4497e945de6b 39 float th1 = mEnc1.getAngle();
alexc89 46:4497e945de6b 40 float th2 = mEnc2.getAngle()-pi/2;
alexc89 46:4497e945de6b 41 double y = l1+l2*sin(th1)+l3*sin(th2-th1)+ l2*sin(th1);
alexc89 46:4497e945de6b 42 double x = l2*cos(th1)+l3*cos(th2-th1)+ l2*cos(th1);
alexc89 46:4497e945de6b 43 float length = pow(pow(x,2.0)+pow(y,2.0),0.5);
alexc89 46:4497e945de6b 44 int ks = 0.1;
alexc89 46:4497e945de6b 45 float mag = (initLen-length)*ks;
alexc89 46:4497e945de6b 46 //Apply Forward Kinematics
alexc89 46:4497e945de6b 47 Point output = Point(-mag*x/length,-mag*y/length,0);
alexc89 46:4497e945de6b 48 Joints dtorque = invKinBody(output);
alexc89 46:4497e945de6b 49 //Apply Motor Torque Control.
alexc89 46:4497e945de6b 50 m1.setTorque(dtorque.t1);
alexc89 46:4497e945de6b 51 m2.setTorque(dtorque.t2);
alexc89 46:4497e945de6b 52 return 1;
alexc89 46:4497e945de6b 53 }
alexc89 46:4497e945de6b 54 int FLIGHT(float initM1, float initM2){
alexc89 46:4497e945de6b 55 //Pos Control
alexc89 46:4497e945de6b 56 m1.setPos(initM1);
alexc89 46:4497e945de6b 57 m2.setPos(initM2);
alexc89 46:4497e945de6b 58 //Awesome.
alexc89 46:4497e945de6b 59 return 1;
alexc89 46:4497e945de6b 60 }
calamaridudeman 37:bf257a0154db 61 int main() {
calamaridudeman 43:68faf056ed5c 62 Point p1(0,-.1,0);
calamaridudeman 43:68faf056ed5c 63 Point p2(0,-.6,0);
calamaridudeman 43:68faf056ed5c 64 Point p3(0,-.1,0);
calamaridudeman 43:68faf056ed5c 65 Point p[3]={p1, p2, p3};
calamaridudeman 43:68faf056ed5c 66 BezCurve curve(p, 3);
calamaridudeman 43:68faf056ed5c 67 curve.startCurve();
alexc89 45:0db0fc9f77b1 68 m1.setPos(0);
alexc89 46:4497e945de6b 69 m2.setPos(0);
alexc89 46:4497e945de6b 70
calamaridudeman 43:68faf056ed5c 71 kankan.start();
alexc89 46:4497e945de6b 72 //Initalize Control Varialbes
alexc89 45:0db0fc9f77b1 73 int Phase = 0;//Flight Phase
alexc89 46:4497e945de6b 74 int initLen = 0.17;
alexc89 46:4497e945de6b 75 float initM1 = -0.5;
alexc89 46:4497e945de6b 76 float initM2 = 0.5;
alexc89 46:4497e945de6b 77
alexc89 45:0db0fc9f77b1 78 while(true){
alexc89 45:0db0fc9f77b1 79 if (Phase == 0){//Flight Phase
alexc89 46:4497e945de6b 80 //Uses PD to Control
alexc89 46:4497e945de6b 81 //FLIGHT(initM1, initM2);
alexc89 46:4497e945de6b 82 m1.setPos(-0.5);
alexc89 45:0db0fc9f77b1 83 if(landDetection()){
alexc89 45:0db0fc9f77b1 84 //Landed going to Phase 1
alexc89 46:4497e945de6b 85 //Record initial Length.
alexc89 45:0db0fc9f77b1 86 Phase =1;
alexc89 46:4497e945de6b 87 float th1 = mEnc1.getAngle();
alexc89 46:4497e945de6b 88 float th2 = mEnc2.getAngle()-pi/2;
alexc89 46:4497e945de6b 89 double y = l1+l2*sin(th1)+l3*sin(th2-th1)+ l2*sin(th1);
alexc89 46:4497e945de6b 90 double x = l2*cos(th1)+l3*cos(th2-th1)+ l2*cos(th1);
alexc89 46:4497e945de6b 91 initLen = pow(pow(x,2.0)+pow(y,2.0),0.5);
alexc89 45:0db0fc9f77b1 92 }
alexc89 45:0db0fc9f77b1 93 }else{ //Land Phase
alexc89 46:4497e945de6b 94 //Uses SLIP Model to Control
alexc89 46:4497e945de6b 95 //SLIP(initLen);
alexc89 46:4497e945de6b 96 m1.setPos(-1);
alexc89 45:0db0fc9f77b1 97 if(!landDetection()){
alexc89 45:0db0fc9f77b1 98 //Lift Up going to Phase 0
alexc89 45:0db0fc9f77b1 99 Phase =0;
alexc89 45:0db0fc9f77b1 100 }
alexc89 45:0db0fc9f77b1 101
alexc89 45:0db0fc9f77b1 102 }
alexc89 45:0db0fc9f77b1 103 }
calamaridudeman 43:68faf056ed5c 104 wait(3);
calamaridudeman 43:68faf056ed5c 105
calamaridudeman 43:68faf056ed5c 106 /*while(!curve.isDone()){
calamaridudeman 43:68faf056ed5c 107 //kankan.testEncoders(pc);
calamaridudeman 43:68faf056ed5c 108 curve.incrementAlpha();
calamaridudeman 43:68faf056ed5c 109 Point end = curve.getPoint();
calamaridudeman 43:68faf056ed5c 110
calamaridudeman 43:68faf056ed5c 111 Joints motorset = invKinBody(end);
calamaridudeman 43:68faf056ed5c 112 //pc.printf("%f, %f, %f, %f\n", motorset.t1, motorset.t2, end.x, end.y);
calamaridudeman 43:68faf056ed5c 113
calamaridudeman 43:68faf056ed5c 114 m1.setPos(motorset.t1);
calamaridudeman 43:68faf056ed5c 115 m2.setPos(motorset.t2);
calamaridudeman 43:68faf056ed5c 116
calamaridudeman 43:68faf056ed5c 117 wait(.05);
calamaridudeman 43:68faf056ed5c 118 }
calamaridudeman 43:68faf056ed5c 119 */
calamaridudeman 23:112c0be5a7f3 120
calamaridudeman 43:68faf056ed5c 121 /*
calamaridudeman 43:68faf056ed5c 122 kankan.start();
calamaridudeman 43:68faf056ed5c 123 wait(1);
calamaridudeman 43:68faf056ed5c 124 //m1.setPos(-3.1415/6);
calamaridudeman 43:68faf056ed5c 125 //m2.setPos(3.1415/6);
calamaridudeman 43:68faf056ed5c 126 //m1.setTorque(2);
calamaridudeman 37:bf257a0154db 127
calamaridudeman 43:68faf056ed5c 128 //kankan.setPoint(Point(0,-.3,0));
calamaridudeman 43:68faf056ed5c 129 wait(5);
calamaridudeman 43:68faf056ed5c 130 */
calamaridudeman 43:68faf056ed5c 131 m1.stop();
calamaridudeman 43:68faf056ed5c 132 m2.stop();
calamaridudeman 43:68faf056ed5c 133
calamaridudeman 43:68faf056ed5c 134 /*Forward=1;
calamaridudeman 43:68faf056ed5c 135 Backward=0;
calamaridudeman 43:68faf056ed5c 136 pwmOut.period_us(500);
calamaridudeman 43:68faf056ed5c 137
calamaridudeman 43:68faf056ed5c 138
calamaridudeman 43:68faf056ed5c 139 pwmOut.write(.15);
calamaridudeman 43:68faf056ed5c 140
calamaridudeman 43:68faf056ed5c 141 wait(5);
calamaridudeman 43:68faf056ed5c 142 pwmOut.write(0);
calamaridudeman 43:68faf056ed5c 143
calamaridudeman 43:68faf056ed5c 144 */
calamaridudeman 37:bf257a0154db 145
calamaridudeman 37:bf257a0154db 146 //kankan.zero();
calamaridudeman 22:4d85d989af08 147 }