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@53:978b7fa74080, 2013-12-02 (annotated)

Committer:
calamaridudeman
Date:
Mon Dec 02 05:16:26 2013 +0000
Revision:
53:978b7fa74080
Parent:
49:3aaa790800ad
Child:
54:17ea4b3c80de
shin works too, now to get event detection

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 43:68faf056ed5c 14 QEI bEnc1(p27, p28, p29, 500, QEI::X4_ENCODING); //track offset:-2.236814
calamaridudeman 43:68faf056ed5c 15 QEI bEnc2(p5, p6, p7, 500, 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
alexc89 45:0db0fc9f77b1 27 int landDetection(){
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.
alexc89 46:4497e945de6b 31 float th3 = bEnc1.getAngle();
alexc89 45:0db0fc9f77b1 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;
alexc89 46:4497e945de6b 35 float b = l1+l2*sin(th1)+l3*sin(th2-th1)+ l2*sin(th1);
alexc89 45:0db0fc9f77b1 36 return a<b;
alexc89 45:0db0fc9f77b1 37 }
calamaridudeman 43:68faf056ed5c 38
alexc89 46:4497e945de6b 39 int SLIP(float initLen){//SlIP Model
alexc89 46:4497e945de6b 40 //Get Length
alexc89 46:4497e945de6b 41 float th1 = mEnc1.getAngle();
alexc89 46:4497e945de6b 42 float th2 = mEnc2.getAngle()-pi/2;
alexc89 46:4497e945de6b 43 double y = l1+l2*sin(th1)+l3*sin(th2-th1)+ l2*sin(th1);
alexc89 46:4497e945de6b 44 double x = l2*cos(th1)+l3*cos(th2-th1)+ l2*cos(th1);
sherryxy 48:8f0e007bd305 45 //float length = pow(pow(x,2.0)+pow(y,2.0),0.5);
sherryxy 48:8f0e007bd305 46 int ks = 0.5;
sherryxy 48:8f0e007bd305 47 //float mag = (initLen-length)*ks;
alexc89 46:4497e945de6b 48 //Apply Forward Kinematics
sherryxy 48:8f0e007bd305 49 //Point output = Point(-mag*x/length,-mag*y/length,0);
sherryxy 48:8f0e007bd305 50 Point output(-(1+ks)*x,-(1+ks)*y,0); //try just position scaling for slip
sherryxy 48:8f0e007bd305 51 Joints dtheta = invKinBody(output);
sherryxy 48:8f0e007bd305 52 m1.setPos(dtheta.t1);
sherryxy 48:8f0e007bd305 53 m2.setPos(dtheta.t2);
sherryxy 48:8f0e007bd305 54 //Joints dtorque = invKinBody(output);
alexc89 46:4497e945de6b 55 //Apply Motor Torque Control.
sherryxy 48:8f0e007bd305 56 //m1.setTorque(dtheta.t1);
sherryxy 48:8f0e007bd305 57 //m2.setTorque(dtheta.t2);
alexc89 46:4497e945de6b 58 return 1;
alexc89 46:4497e945de6b 59 }
alexc89 46:4497e945de6b 60 int FLIGHT(float initM1, float initM2){
alexc89 46:4497e945de6b 61 //Pos Control
alexc89 46:4497e945de6b 62 m1.setPos(initM1);
alexc89 46:4497e945de6b 63 m2.setPos(initM2);
alexc89 46:4497e945de6b 64 //Awesome.
alexc89 46:4497e945de6b 65 return 1;
alexc89 46:4497e945de6b 66 }
calamaridudeman 37:bf257a0154db 67 int main() {
sherryxy 48:8f0e007bd305 68 kankan.start();
sherryxy 48:8f0e007bd305 69 kankan.zero();
sherryxy 48:8f0e007bd305 70
sherryxy 49:3aaa790800ad 71 while(1){
sherryxy 49:3aaa790800ad 72 kankan.testEncoders(pc);
calamaridudeman 53:978b7fa74080 73 wait(0.01);
sherryxy 49:3aaa790800ad 74 }
sherryxy 49:3aaa790800ad 75
sherryxy 48:8f0e007bd305 76 /* Point p1(0,-.1,0);
calamaridudeman 43:68faf056ed5c 77 Point p2(0,-.6,0);
calamaridudeman 43:68faf056ed5c 78 Point p3(0,-.1,0);
calamaridudeman 43:68faf056ed5c 79 Point p[3]={p1, p2, p3};
calamaridudeman 43:68faf056ed5c 80 BezCurve curve(p, 3);
calamaridudeman 43:68faf056ed5c 81 curve.startCurve();
alexc89 45:0db0fc9f77b1 82 m1.setPos(0);
alexc89 46:4497e945de6b 83 m2.setPos(0);
alexc89 46:4497e945de6b 84
calamaridudeman 43:68faf056ed5c 85 kankan.start();
sherryxy 48:8f0e007bd305 86 */
sherryxy 48:8f0e007bd305 87
alexc89 46:4497e945de6b 88 //Initalize Control Varialbes
alexc89 45:0db0fc9f77b1 89 int Phase = 0;//Flight Phase
sherryxy 48:8f0e007bd305 90 int initLen = 0.08;//0.17;
sherryxy 48:8f0e007bd305 91 //float initM1 = -0.5;
sherryxy 48:8f0e007bd305 92 //float initM2 = 0.5;
alexc89 46:4497e945de6b 93
alexc89 45:0db0fc9f77b1 94 while(true){
alexc89 45:0db0fc9f77b1 95 if (Phase == 0){//Flight Phase
alexc89 46:4497e945de6b 96 //Uses PD to Control
alexc89 46:4497e945de6b 97 //FLIGHT(initM1, initM2);
sherryxy 48:8f0e007bd305 98 //m1.setPos(-0.5);
sherryxy 48:8f0e007bd305 99 led1=0;
sherryxy 48:8f0e007bd305 100 kankan.setPoint(Point(0,-.08,0));
alexc89 45:0db0fc9f77b1 101 if(landDetection()){
alexc89 45:0db0fc9f77b1 102 //Landed going to Phase 1
sherryxy 48:8f0e007bd305 103 //Record initial Length.
alexc89 45:0db0fc9f77b1 104 Phase =1;
alexc89 46:4497e945de6b 105 float th1 = mEnc1.getAngle();
alexc89 46:4497e945de6b 106 float th2 = mEnc2.getAngle()-pi/2;
alexc89 46:4497e945de6b 107 double y = l1+l2*sin(th1)+l3*sin(th2-th1)+ l2*sin(th1);
alexc89 46:4497e945de6b 108 double x = l2*cos(th1)+l3*cos(th2-th1)+ l2*cos(th1);
sherryxy 48:8f0e007bd305 109 initLen = pow(pow(x,2.0)+pow(y,2.0),0.5);
alexc89 45:0db0fc9f77b1 110 }
alexc89 45:0db0fc9f77b1 111 }else{ //Land Phase
sherryxy 48:8f0e007bd305 112 led1=1; //landing detected
alexc89 46:4497e945de6b 113 //Uses SLIP Model to Control
sherryxy 48:8f0e007bd305 114
sherryxy 48:8f0e007bd305 115 SLIP(initLen);
sherryxy 48:8f0e007bd305 116 //m1.setPos(-1);
alexc89 45:0db0fc9f77b1 117 if(!landDetection()){
alexc89 45:0db0fc9f77b1 118 //Lift Up going to Phase 0
alexc89 45:0db0fc9f77b1 119 Phase =0;
alexc89 45:0db0fc9f77b1 120 }
alexc89 45:0db0fc9f77b1 121
alexc89 45:0db0fc9f77b1 122 }
alexc89 45:0db0fc9f77b1 123 }
calamaridudeman 43:68faf056ed5c 124 wait(3);
calamaridudeman 43:68faf056ed5c 125
calamaridudeman 43:68faf056ed5c 126 /*while(!curve.isDone()){
calamaridudeman 43:68faf056ed5c 127 //kankan.testEncoders(pc);
calamaridudeman 43:68faf056ed5c 128 curve.incrementAlpha();
calamaridudeman 43:68faf056ed5c 129 Point end = curve.getPoint();
calamaridudeman 43:68faf056ed5c 130
calamaridudeman 43:68faf056ed5c 131 Joints motorset = invKinBody(end);
calamaridudeman 43:68faf056ed5c 132 //pc.printf("%f, %f, %f, %f\n", motorset.t1, motorset.t2, end.x, end.y);
calamaridudeman 43:68faf056ed5c 133
calamaridudeman 43:68faf056ed5c 134 m1.setPos(motorset.t1);
calamaridudeman 43:68faf056ed5c 135 m2.setPos(motorset.t2);
calamaridudeman 43:68faf056ed5c 136
calamaridudeman 43:68faf056ed5c 137 wait(.05);
calamaridudeman 43:68faf056ed5c 138 }
calamaridudeman 43:68faf056ed5c 139 */
calamaridudeman 23:112c0be5a7f3 140
calamaridudeman 43:68faf056ed5c 141 /*
calamaridudeman 43:68faf056ed5c 142 kankan.start();
calamaridudeman 43:68faf056ed5c 143 wait(1);
calamaridudeman 43:68faf056ed5c 144 //m1.setPos(-3.1415/6);
calamaridudeman 43:68faf056ed5c 145 //m2.setPos(3.1415/6);
calamaridudeman 43:68faf056ed5c 146 //m1.setTorque(2);
calamaridudeman 37:bf257a0154db 147
calamaridudeman 43:68faf056ed5c 148 //kankan.setPoint(Point(0,-.3,0));
calamaridudeman 43:68faf056ed5c 149 wait(5);
calamaridudeman 43:68faf056ed5c 150 */
calamaridudeman 43:68faf056ed5c 151 m1.stop();
calamaridudeman 43:68faf056ed5c 152 m2.stop();
calamaridudeman 43:68faf056ed5c 153
calamaridudeman 43:68faf056ed5c 154 /*Forward=1;
calamaridudeman 43:68faf056ed5c 155 Backward=0;
calamaridudeman 43:68faf056ed5c 156 pwmOut.period_us(500);
calamaridudeman 43:68faf056ed5c 157
calamaridudeman 43:68faf056ed5c 158
calamaridudeman 43:68faf056ed5c 159 pwmOut.write(.15);
calamaridudeman 43:68faf056ed5c 160
calamaridudeman 43:68faf056ed5c 161 wait(5);
calamaridudeman 43:68faf056ed5c 162 pwmOut.write(0);
calamaridudeman 43:68faf056ed5c 163
calamaridudeman 43:68faf056ed5c 164 */
calamaridudeman 37:bf257a0154db 165
calamaridudeman 37:bf257a0154db 166 //kankan.zero();
calamaridudeman 22:4d85d989af08 167 }