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@55:ff84fbdfd1d1, 2013-12-02 (annotated)

Committer:
calamaridudeman
Date:
Mon Dec 02 23:15:15 2013 +0000
Revision:
55:ff84fbdfd1d1
Parent:
54:17ea4b3c80de
Child:
56:7015e2e79ea7
everything except SLIP/flight stuff works

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 46:4497e945de6b 26 int SLIP(float initLen){//SlIP Model
alexc89 46:4497e945de6b 27 //Get Length
calamaridudeman 55:ff84fbdfd1d1 28 double y = kankan.getPoint().y;
calamaridudeman 55:ff84fbdfd1d1 29 double x = kankan.getPoint().x;
sherryxy 48:8f0e007bd305 30 //float length = pow(pow(x,2.0)+pow(y,2.0),0.5);
sherryxy 48:8f0e007bd305 31 int ks = 0.5;
sherryxy 48:8f0e007bd305 32 //float mag = (initLen-length)*ks;
alexc89 46:4497e945de6b 33 //Apply Forward Kinematics
calamaridudeman 55:ff84fbdfd1d1 34 kankan.setPoint(Point((1+ks)*x,(1+ks)*y,0));
sherryxy 48:8f0e007bd305 35 //Point output = Point(-mag*x/length,-mag*y/length,0);
calamaridudeman 55:ff84fbdfd1d1 36 //Point output((1+ks)*x,(1+ks)*y,0); //try just position scaling for slip
calamaridudeman 55:ff84fbdfd1d1 37 //Joints dtheta = invKinBody(output);
calamaridudeman 55:ff84fbdfd1d1 38 //m1.setPos(dtheta.t1);
calamaridudeman 55:ff84fbdfd1d1 39 //m2.setPos(dtheta.t2);
alexc89 46:4497e945de6b 40 return 1;
alexc89 46:4497e945de6b 41 }
alexc89 46:4497e945de6b 42 int FLIGHT(float initM1, float initM2){
alexc89 46:4497e945de6b 43 //Pos Control
alexc89 46:4497e945de6b 44 m1.setPos(initM1);
alexc89 46:4497e945de6b 45 m2.setPos(initM2);
alexc89 46:4497e945de6b 46 //Awesome.
alexc89 46:4497e945de6b 47 return 1;
alexc89 46:4497e945de6b 48 }
calamaridudeman 37:bf257a0154db 49 int main() {
calamaridudeman 55:ff84fbdfd1d1 50 kankan.start();
calamaridudeman 55:ff84fbdfd1d1 51 kankan.zero();
calamaridudeman 55:ff84fbdfd1d1 52
calamaridudeman 55:ff84fbdfd1d1 53 Point p1(0,-.1,0);
calamaridudeman 43:68faf056ed5c 54 Point p2(0,-.6,0);
calamaridudeman 43:68faf056ed5c 55 Point p3(0,-.1,0);
calamaridudeman 43:68faf056ed5c 56 Point p[3]={p1, p2, p3};
calamaridudeman 43:68faf056ed5c 57 BezCurve curve(p, 3);
calamaridudeman 43:68faf056ed5c 58 curve.startCurve();
calamaridudeman 55:ff84fbdfd1d1 59
alexc89 46:4497e945de6b 60
calamaridudeman 55:ff84fbdfd1d1 61 //kankan.start();
calamaridudeman 55:ff84fbdfd1d1 62 /*
sherryxy 48:8f0e007bd305 63
alexc89 46:4497e945de6b 64 //Initalize Control Varialbes
alexc89 45:0db0fc9f77b1 65 int Phase = 0;//Flight Phase
sherryxy 48:8f0e007bd305 66 int initLen = 0.08;//0.17;
sherryxy 48:8f0e007bd305 67 //float initM1 = -0.5;
sherryxy 48:8f0e007bd305 68 //float initM2 = 0.5;
alexc89 46:4497e945de6b 69
alexc89 45:0db0fc9f77b1 70 while(true){
alexc89 45:0db0fc9f77b1 71 if (Phase == 0){//Flight Phase
alexc89 46:4497e945de6b 72 //Uses PD to Control
alexc89 46:4497e945de6b 73 //FLIGHT(initM1, initM2);
sherryxy 48:8f0e007bd305 74 //m1.setPos(-0.5);
sherryxy 48:8f0e007bd305 75 led1=0;
calamaridudeman 55:ff84fbdfd1d1 76 kankan.setPoint(Point(0,-.15,0));
calamaridudeman 55:ff84fbdfd1d1 77 if(kankan.landDetection()){
alexc89 45:0db0fc9f77b1 78 //Landed going to Phase 1
sherryxy 48:8f0e007bd305 79 //Record initial Length.
alexc89 45:0db0fc9f77b1 80 Phase =1;
calamaridudeman 55:ff84fbdfd1d1 81 initLen = sqrt(pow(kankan.getPoint().x,2)+pow(kankan.getPoint().y,2));
alexc89 45:0db0fc9f77b1 82 }
alexc89 45:0db0fc9f77b1 83 }else{ //Land Phase
sherryxy 48:8f0e007bd305 84 led1=1; //landing detected
alexc89 46:4497e945de6b 85 //Uses SLIP Model to Control
sherryxy 48:8f0e007bd305 86
sherryxy 48:8f0e007bd305 87 SLIP(initLen);
sherryxy 48:8f0e007bd305 88 //m1.setPos(-1);
calamaridudeman 55:ff84fbdfd1d1 89 if(!kankan.landDetection()){
alexc89 45:0db0fc9f77b1 90 //Lift Up going to Phase 0
alexc89 45:0db0fc9f77b1 91 Phase =0;
alexc89 45:0db0fc9f77b1 92 }
alexc89 45:0db0fc9f77b1 93
alexc89 45:0db0fc9f77b1 94 }
alexc89 45:0db0fc9f77b1 95 }
calamaridudeman 43:68faf056ed5c 96 wait(3);
calamaridudeman 55:ff84fbdfd1d1 97 */
calamaridudeman 55:ff84fbdfd1d1 98 while(!curve.isDone()){
calamaridudeman 43:68faf056ed5c 99 //kankan.testEncoders(pc);
calamaridudeman 43:68faf056ed5c 100 curve.incrementAlpha();
calamaridudeman 43:68faf056ed5c 101 Point end = curve.getPoint();
calamaridudeman 43:68faf056ed5c 102
calamaridudeman 55:ff84fbdfd1d1 103 kankan.setPoint(end);
calamaridudeman 43:68faf056ed5c 104
calamaridudeman 43:68faf056ed5c 105 wait(.05);
calamaridudeman 43:68faf056ed5c 106 }
calamaridudeman 23:112c0be5a7f3 107
calamaridudeman 55:ff84fbdfd1d1 108
calamaridudeman 43:68faf056ed5c 109 m1.stop();
calamaridudeman 43:68faf056ed5c 110 m2.stop();
calamaridudeman 55:ff84fbdfd1d1 111
calamaridudeman 22:4d85d989af08 112 }