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

src/kangaroo.cpp@56:7015e2e79ea7, 2013-12-03 (annotated)

Committer:
calamaridudeman
Date:
Tue Dec 03 02:49:30 2013 +0000
Revision:
56:7015e2e79ea7
Parent:
55:ff84fbdfd1d1
Child:
57:dfea5d24d650
force control works :3

Who changed what in which revision?

UserRevisionLine numberNew contents of line
calamaridudeman 37:bf257a0154db 1 #include "mbed.h"
calamaridudeman 37:bf257a0154db 2 #include "include/kangaroo.hpp"
calamaridudeman 37:bf257a0154db 3
calamaridudeman 37:bf257a0154db 4
calamaridudeman 43:68faf056ed5c 5
calamaridudeman 37:bf257a0154db 6 Kangaroo::Kangaroo(Motor &M1, Motor &M2, QEI &e1, QEI &e2):enc1(e1),enc2(e2),m1(M1),m2(M2),led1(LED1),led2(LED2),led3(LED3),led4(LED4){
calamaridudeman 37:bf257a0154db 7
calamaridudeman 55:ff84fbdfd1d1 8 joint_h=0;
calamaridudeman 55:ff84fbdfd1d1 9 leg_y = 0;
calamaridudeman 55:ff84fbdfd1d1 10
calamaridudeman 37:bf257a0154db 11 }
calamaridudeman 37:bf257a0154db 12
calamaridudeman 37:bf257a0154db 13 void Kangaroo::zero(){
calamaridudeman 43:68faf056ed5c 14 m1.start();
calamaridudeman 43:68faf056ed5c 15 m2.start();
calamaridudeman 43:68faf056ed5c 16
calamaridudeman 55:ff84fbdfd1d1 17 m1.setVel(.5);//use in final
calamaridudeman 55:ff84fbdfd1d1 18 m2.setVel(.3);
calamaridudeman 53:978b7fa74080 19 //m2.setPos(3.1415/5);//bring legs to hard stop
calamaridudeman 43:68faf056ed5c 20
calamaridudeman 37:bf257a0154db 21 led4=1;
calamaridudeman 53:978b7fa74080 22 wait(5.0);
calamaridudeman 37:bf257a0154db 23
sherryxy 50:510ce69c2a10 24 //m1.zero();
sherryxy 50:510ce69c2a10 25 //m2.zero();
calamaridudeman 53:978b7fa74080 26 m1.calibAngle(0.0);
calamaridudeman 53:978b7fa74080 27 m2.calibAngle(2.44);
calamaridudeman 43:68faf056ed5c 28
calamaridudeman 55:ff84fbdfd1d1 29 wait(.1);
calamaridudeman 55:ff84fbdfd1d1 30
sherryxy 48:8f0e007bd305 31
calamaridudeman 55:ff84fbdfd1d1 32 m1.setPos(-3.1415/12);
calamaridudeman 55:ff84fbdfd1d1 33 m2.setPos(3.1415/2);
calamaridudeman 43:68faf056ed5c 34
calamaridudeman 37:bf257a0154db 35 enc1.reset();
calamaridudeman 37:bf257a0154db 36 enc2.reset();
calamaridudeman 37:bf257a0154db 37 led2=1;
calamaridudeman 55:ff84fbdfd1d1 38 while(((enc1.getRevolutions()==0)||(enc2.getRevolutions()==0))){
calamaridudeman 37:bf257a0154db 39 wait(0.1);
calamaridudeman 55:ff84fbdfd1d1 40 }
calamaridudeman 37:bf257a0154db 41
calamaridudeman 55:ff84fbdfd1d1 42 landDetection();
calamaridudeman 37:bf257a0154db 43 led1=1;
calamaridudeman 43:68faf056ed5c 44
sherryxy 49:3aaa790800ad 45 //setPoint(Point(0,-.08,0));
calamaridudeman 55:ff84fbdfd1d1 46 wait(3);
sherryxy 48:8f0e007bd305 47 led3=0;
sherryxy 48:8f0e007bd305 48 led1=0;
sherryxy 48:8f0e007bd305 49 led2=0;
sherryxy 48:8f0e007bd305 50 led4=0;
sherryxy 48:8f0e007bd305 51 //m1.stop();
sherryxy 48:8f0e007bd305 52 //m2.stop();
calamaridudeman 37:bf257a0154db 53 }
calamaridudeman 37:bf257a0154db 54
calamaridudeman 55:ff84fbdfd1d1 55 float Kangaroo::jointHeight(){
calamaridudeman 55:ff84fbdfd1d1 56 return joint_h;
calamaridudeman 55:ff84fbdfd1d1 57 }
calamaridudeman 55:ff84fbdfd1d1 58
calamaridudeman 55:ff84fbdfd1d1 59 float Kangaroo::legY(){
calamaridudeman 55:ff84fbdfd1d1 60 return leg_y;
calamaridudeman 55:ff84fbdfd1d1 61 }
calamaridudeman 55:ff84fbdfd1d1 62
calamaridudeman 37:bf257a0154db 63 void Kangaroo::testEncoders(Serial &pc){
calamaridudeman 37:bf257a0154db 64 //pc.printf("hello world\n");
sherryxy 50:510ce69c2a10 65 //pc.printf("x: %f y: %f\n", getPoint().x, getPoint().y);
calamaridudeman 37:bf257a0154db 66
calamaridudeman 56:7015e2e79ea7 67 pc.printf("m1: %f m2: %f\n",dt1, dt2);
calamaridudeman 43:68faf056ed5c 68 }
calamaridudeman 43:68faf056ed5c 69
calamaridudeman 55:ff84fbdfd1d1 70 int Kangaroo::landDetection(){
calamaridudeman 55:ff84fbdfd1d1 71 //Helper function to Detect If we have landed.
calamaridudeman 55:ff84fbdfd1d1 72 //NO Input
calamaridudeman 55:ff84fbdfd1d1 73 //Output: 0 = Flight Phase. 1 = Ground Phase.
calamaridudeman 55:ff84fbdfd1d1 74 float th3 = getAngle1();
calamaridudeman 55:ff84fbdfd1d1 75 joint_h = lg*sin(th3)+h;
calamaridudeman 55:ff84fbdfd1d1 76 float th4 = getAngle2();
calamaridudeman 55:ff84fbdfd1d1 77 leg_y = -1*(-l1+getPoint().y)*cos(th4);
calamaridudeman 55:ff84fbdfd1d1 78 return joint_h<(leg_y+.013);
calamaridudeman 55:ff84fbdfd1d1 79 }
calamaridudeman 55:ff84fbdfd1d1 80
sherryxy 49:3aaa790800ad 81 Point Kangaroo::getPoint(){
calamaridudeman 55:ff84fbdfd1d1 82 float x = (l2+l4)*cos(m1.getAngle())+(l3)*cos(m1.getAngle()+m2.getAngle()-3.1415);
calamaridudeman 55:ff84fbdfd1d1 83 float y = (l2+l4)*sin(m1.getAngle())+(l3)*sin(m1.getAngle()+m2.getAngle()-3.1415);
sherryxy 49:3aaa790800ad 84
sherryxy 49:3aaa790800ad 85 return Point(x,y,0);
sherryxy 49:3aaa790800ad 86 }
sherryxy 48:8f0e007bd305 87
calamaridudeman 54:17ea4b3c80de 88 float Kangaroo::getAngle2(){
calamaridudeman 55:ff84fbdfd1d1 89 return (enc2.getAngle())-1.65;
calamaridudeman 54:17ea4b3c80de 90 }
calamaridudeman 54:17ea4b3c80de 91
calamaridudeman 54:17ea4b3c80de 92
calamaridudeman 54:17ea4b3c80de 93 float Kangaroo::getAngle1(){
calamaridudeman 54:17ea4b3c80de 94 return -(enc1.getAngle()-0.5372);
calamaridudeman 43:68faf056ed5c 95 }
calamaridudeman 43:68faf056ed5c 96
calamaridudeman 43:68faf056ed5c 97 void Kangaroo::setPoint(Point p){
calamaridudeman 43:68faf056ed5c 98
calamaridudeman 43:68faf056ed5c 99 Joints motors = invKinBody(p);
calamaridudeman 43:68faf056ed5c 100 m1.setPos(motors.t1);
calamaridudeman 43:68faf056ed5c 101 m2.setPos(motors.t2);
calamaridudeman 43:68faf056ed5c 102
calamaridudeman 43:68faf056ed5c 103 }
calamaridudeman 43:68faf056ed5c 104
calamaridudeman 56:7015e2e79ea7 105 Joints Kangaroo::jacobianT(Point p){
calamaridudeman 56:7015e2e79ea7 106 float t1=m1.getAngle();
calamaridudeman 56:7015e2e79ea7 107 float t2=m2.getAngle();
calamaridudeman 56:7015e2e79ea7 108
calamaridudeman 56:7015e2e79ea7 109 float tor1 = ((l2+l4)*(-sin(t1))+l3*(-sin(t1+t2-3.1415)))*p.x+((l2+l4)*(cos(t1))+l3*(cos(t1+t2-3.1415)))*p.y;
calamaridudeman 56:7015e2e79ea7 110 float tor2 = (l3*(-sin(t1+t2-3.1415)))*p.x+(l3*(cos(t1+t2-3.1415)))*p.y;
calamaridudeman 56:7015e2e79ea7 111 return Joints(tor1,tor2);
calamaridudeman 56:7015e2e79ea7 112 }
calamaridudeman 56:7015e2e79ea7 113
calamaridudeman 56:7015e2e79ea7 114 void Kangaroo::slip(float k, Point desir, Serial &pc){
calamaridudeman 56:7015e2e79ea7 115 Point now = getPoint();
calamaridudeman 56:7015e2e79ea7 116 //pc.printf("nx: %f ny: %f dx: %f dy: %f ", now.x, now.y, desir.x, desir.y);
calamaridudeman 56:7015e2e79ea7 117 float fy = (now.y-desir.y)*-k;
calamaridudeman 56:7015e2e79ea7 118 float fx = (now.x-desir.x)*-k;
calamaridudeman 56:7015e2e79ea7 119
calamaridudeman 56:7015e2e79ea7 120 Point force = Point(fx, fy, 0);
calamaridudeman 56:7015e2e79ea7 121
calamaridudeman 56:7015e2e79ea7 122 Joints torq = jacobianT(force);
calamaridudeman 56:7015e2e79ea7 123 //pc.printf("t1: %f t2: %f\n", torq.t1, torq.t2);
calamaridudeman 56:7015e2e79ea7 124 m1.setTorque(torq.t1);
calamaridudeman 56:7015e2e79ea7 125 m2.setTorque(torq.t2);
calamaridudeman 56:7015e2e79ea7 126 //dt1=torq.t1;
calamaridudeman 56:7015e2e79ea7 127 //dt2=torq.t2;
calamaridudeman 56:7015e2e79ea7 128 }
calamaridudeman 56:7015e2e79ea7 129
calamaridudeman 43:68faf056ed5c 130 void Kangaroo::flightPath(){
calamaridudeman 43:68faf056ed5c 131
calamaridudeman 43:68faf056ed5c 132 }
calamaridudeman 43:68faf056ed5c 133
calamaridudeman 43:68faf056ed5c 134 void Kangaroo::start(){
calamaridudeman 43:68faf056ed5c 135 m1.start();
calamaridudeman 43:68faf056ed5c 136 m2.start();
calamaridudeman 43:68faf056ed5c 137 }