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@45:0db0fc9f77b1, 2013-12-01 (annotated)

Committer:
alexc89
Date:
Sun Dec 01 07:23:22 2013 +0000
Revision:
45:0db0fc9f77b1
Parent:
44:3566c5699ba6
Child:
46:4497e945de6b
Pos and Velocity Control Checked.   Phase detection and control structure implemented and checked.

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);
alexc89 45:0db0fc9f77b1 5 //Tick = 1200
calamaridudeman 37:bf257a0154db 6 QEI mEnc1(p25, p26, NC, 1200, QEI::X4_ENCODING); //hip
calamaridudeman 37:bf257a0154db 7 QEI mEnc2(p23, p24, NC, 1200, QEI::X4_ENCODING);//knee
calamaridudeman 37:bf257a0154db 8
calamaridudeman 43:68faf056ed5c 9 Motor m1(p15,p17,p18,p21,mEnc2);//hip p17 high is CCW
calamaridudeman 43:68faf056ed5c 10 Motor m2(p16,p19,p20,p22,mEnc1);//knee p19 high is CCW
calamaridudeman 25:8a34b8d6cc6e 11
alexc89 45:0db0fc9f77b1 12
alexc89 45:0db0fc9f77b1 13 //Tick = 500
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 45:0db0fc9f77b1 31 float th3 = bEnc1.getAngle()*6.283;
alexc89 45:0db0fc9f77b1 32 float a = lg*sin(-th3)+h;
alexc89 45:0db0fc9f77b1 33 float th1 = mEnc1.getAngle()*6.283;
alexc89 45:0db0fc9f77b1 34 float th2 = mEnc2.getAngle()*6.283-pi/2;
alexc89 45:0db0fc9f77b1 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
calamaridudeman 37:bf257a0154db 39 int main() {
calamaridudeman 43:68faf056ed5c 40 Point p1(0,-.1,0);
calamaridudeman 43:68faf056ed5c 41 Point p2(0,-.6,0);
calamaridudeman 43:68faf056ed5c 42 Point p3(0,-.1,0);
calamaridudeman 43:68faf056ed5c 43 Point p[3]={p1, p2, p3};
calamaridudeman 43:68faf056ed5c 44 BezCurve curve(p, 3);
calamaridudeman 43:68faf056ed5c 45 curve.startCurve();
alexc89 45:0db0fc9f77b1 46 m1.setPos(0);
alexc89 45:0db0fc9f77b1 47 m2.setPos(0);
calamaridudeman 43:68faf056ed5c 48 kankan.start();
alexc89 45:0db0fc9f77b1 49 int Phase = 0;//Flight Phase
alexc89 45:0db0fc9f77b1 50 while(true){
alexc89 45:0db0fc9f77b1 51 if (Phase == 0){//Flight Phase
alexc89 45:0db0fc9f77b1 52 //Uses Bezian Curve to Control
alexc89 45:0db0fc9f77b1 53 m1.setPos(-1);
alexc89 45:0db0fc9f77b1 54 if(landDetection()){
alexc89 45:0db0fc9f77b1 55 //Landed going to Phase 1
alexc89 45:0db0fc9f77b1 56 Phase =1;
alexc89 45:0db0fc9f77b1 57 }
alexc89 45:0db0fc9f77b1 58 }else{ //Land Phase
alexc89 45:0db0fc9f77b1 59 //Uses SLIP Model to Control.
alexc89 45:0db0fc9f77b1 60 m1.setPos(-0.5);
alexc89 45:0db0fc9f77b1 61 if(!landDetection()){
alexc89 45:0db0fc9f77b1 62 //Lift Up going to Phase 0
alexc89 45:0db0fc9f77b1 63 Phase =0;
alexc89 45:0db0fc9f77b1 64 }
alexc89 45:0db0fc9f77b1 65
alexc89 45:0db0fc9f77b1 66 }
alexc89 45:0db0fc9f77b1 67 }
calamaridudeman 43:68faf056ed5c 68 wait(3);
calamaridudeman 43:68faf056ed5c 69
calamaridudeman 43:68faf056ed5c 70 /*while(!curve.isDone()){
calamaridudeman 43:68faf056ed5c 71 //kankan.testEncoders(pc);
calamaridudeman 43:68faf056ed5c 72 curve.incrementAlpha();
calamaridudeman 43:68faf056ed5c 73 Point end = curve.getPoint();
calamaridudeman 43:68faf056ed5c 74
calamaridudeman 43:68faf056ed5c 75 Joints motorset = invKinBody(end);
calamaridudeman 43:68faf056ed5c 76 //pc.printf("%f, %f, %f, %f\n", motorset.t1, motorset.t2, end.x, end.y);
calamaridudeman 43:68faf056ed5c 77
calamaridudeman 43:68faf056ed5c 78 m1.setPos(motorset.t1);
calamaridudeman 43:68faf056ed5c 79 m2.setPos(motorset.t2);
calamaridudeman 43:68faf056ed5c 80
calamaridudeman 43:68faf056ed5c 81 wait(.05);
calamaridudeman 43:68faf056ed5c 82 }
calamaridudeman 43:68faf056ed5c 83 */
calamaridudeman 23:112c0be5a7f3 84
calamaridudeman 43:68faf056ed5c 85 /*
calamaridudeman 43:68faf056ed5c 86 kankan.start();
calamaridudeman 43:68faf056ed5c 87 wait(1);
calamaridudeman 43:68faf056ed5c 88 //m1.setPos(-3.1415/6);
calamaridudeman 43:68faf056ed5c 89 //m2.setPos(3.1415/6);
calamaridudeman 43:68faf056ed5c 90 //m1.setTorque(2);
calamaridudeman 37:bf257a0154db 91
calamaridudeman 43:68faf056ed5c 92 //kankan.setPoint(Point(0,-.3,0));
calamaridudeman 43:68faf056ed5c 93 wait(5);
calamaridudeman 43:68faf056ed5c 94 */
calamaridudeman 43:68faf056ed5c 95 m1.stop();
calamaridudeman 43:68faf056ed5c 96 m2.stop();
calamaridudeman 43:68faf056ed5c 97
calamaridudeman 43:68faf056ed5c 98 /*Forward=1;
calamaridudeman 43:68faf056ed5c 99 Backward=0;
calamaridudeman 43:68faf056ed5c 100 pwmOut.period_us(500);
calamaridudeman 43:68faf056ed5c 101
calamaridudeman 43:68faf056ed5c 102
calamaridudeman 43:68faf056ed5c 103 pwmOut.write(.15);
calamaridudeman 43:68faf056ed5c 104
calamaridudeman 43:68faf056ed5c 105 wait(5);
calamaridudeman 43:68faf056ed5c 106 pwmOut.write(0);
calamaridudeman 43:68faf056ed5c 107
calamaridudeman 43:68faf056ed5c 108 */
calamaridudeman 37:bf257a0154db 109
calamaridudeman 37:bf257a0154db 110 //kankan.zero();
calamaridudeman 22:4d85d989af08 111 }