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@54:17ea4b3c80de, 2013-12-02 (annotated)

Committer:
calamaridudeman
Date:
Mon Dec 02 06:54:48 2013 +0000
Revision:
54:17ea4b3c80de
Parent:
53:978b7fa74080
Child:
55:ff84fbdfd1d1
calibrated encoders, here's to hoping they don't get messed up.  getAngles should all return proper angles now.  Ground detection is primed to be double checked in the morning

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