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@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 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 37:bf257a0154db 8 }
calamaridudeman 37:bf257a0154db 9
calamaridudeman 37:bf257a0154db 10 void Kangaroo::zero(){
calamaridudeman 43:68faf056ed5c 11 m1.start();
calamaridudeman 43:68faf056ed5c 12 m2.start();
calamaridudeman 43:68faf056ed5c 13
calamaridudeman 53:978b7fa74080 14 //m1.setVel(.3);//use in final
calamaridudeman 53:978b7fa74080 15 m2.setVel(5);
calamaridudeman 53:978b7fa74080 16 //m2.setPos(3.1415/5);//bring legs to hard stop
calamaridudeman 43:68faf056ed5c 17
calamaridudeman 37:bf257a0154db 18 led4=1;
calamaridudeman 53:978b7fa74080 19 wait(5.0);
calamaridudeman 37:bf257a0154db 20
sherryxy 50:510ce69c2a10 21 //m1.zero();
sherryxy 50:510ce69c2a10 22 //m2.zero();
calamaridudeman 53:978b7fa74080 23 m1.calibAngle(0.0);
calamaridudeman 53:978b7fa74080 24 m2.calibAngle(2.44);
calamaridudeman 43:68faf056ed5c 25
sherryxy 49:3aaa790800ad 26 wait(1);
sherryxy 48:8f0e007bd305 27
calamaridudeman 53:978b7fa74080 28 //m1.setPos(-3.1415/2);
calamaridudeman 54:17ea4b3c80de 29 //m2.setPos(3.1415/2);
calamaridudeman 43:68faf056ed5c 30
calamaridudeman 37:bf257a0154db 31 enc1.reset();
calamaridudeman 37:bf257a0154db 32 enc2.reset();
calamaridudeman 37:bf257a0154db 33 led2=1;
sherryxy 50:510ce69c2a10 34 /*while(((enc1.getRevolutions()==0)||(enc2.getRevolutions()==0))){
calamaridudeman 37:bf257a0154db 35 wait(0.1);
sherryxy 50:510ce69c2a10 36 }*/
calamaridudeman 37:bf257a0154db 37
calamaridudeman 54:17ea4b3c80de 38
calamaridudeman 37:bf257a0154db 39 led1=1;
calamaridudeman 43:68faf056ed5c 40
sherryxy 49:3aaa790800ad 41 //setPoint(Point(0,-.08,0));
sherryxy 50:510ce69c2a10 42 //wait(3);
sherryxy 48:8f0e007bd305 43 led3=0;
sherryxy 48:8f0e007bd305 44 led1=0;
sherryxy 48:8f0e007bd305 45 led2=0;
sherryxy 48:8f0e007bd305 46 led4=0;
sherryxy 48:8f0e007bd305 47 //m1.stop();
sherryxy 48:8f0e007bd305 48 //m2.stop();
calamaridudeman 37:bf257a0154db 49 }
calamaridudeman 37:bf257a0154db 50
calamaridudeman 37:bf257a0154db 51 void Kangaroo::testEncoders(Serial &pc){
calamaridudeman 37:bf257a0154db 52 //pc.printf("hello world\n");
sherryxy 50:510ce69c2a10 53 //pc.printf("x: %f y: %f\n", getPoint().x, getPoint().y);
calamaridudeman 54:17ea4b3c80de 54 pc.printf("me: %f beam: %f\n",getAngle2(), getAngle1());
sherryxy 49:3aaa790800ad 55 //pc.printf("clix1: %i, rots1: %i\n",enc1.getPulses(), enc1.getRevolutions());
calamaridudeman 37:bf257a0154db 56
calamaridudeman 37:bf257a0154db 57 //pc.printf("motor2: %i\n",);
sherryxy 49:3aaa790800ad 58 //pc.printf("clix2: %i, rots2: %i\n",enc2.getPulses(), enc2.getRevolutions());
calamaridudeman 43:68faf056ed5c 59 }
calamaridudeman 43:68faf056ed5c 60
sherryxy 49:3aaa790800ad 61 Point Kangaroo::getPoint(){
sherryxy 49:3aaa790800ad 62 float x = (l2+l4)*cos(m1.getAngle())-(l3)*cos(m1.getAngle()+m2.getAngle());
sherryxy 49:3aaa790800ad 63 float y = (l2+l4)*sin(m1.getAngle())+(l3)*sin(m1.getAngle()+m2.getAngle());
sherryxy 49:3aaa790800ad 64
sherryxy 49:3aaa790800ad 65 return Point(x,y,0);
sherryxy 49:3aaa790800ad 66 }
sherryxy 48:8f0e007bd305 67
calamaridudeman 54:17ea4b3c80de 68 float Kangaroo::getAngle2(){
calamaridudeman 54:17ea4b3c80de 69 return (enc2.getAngle())-1.56;
calamaridudeman 54:17ea4b3c80de 70 }
calamaridudeman 54:17ea4b3c80de 71
calamaridudeman 54:17ea4b3c80de 72
calamaridudeman 54:17ea4b3c80de 73 float Kangaroo::getAngle1(){
calamaridudeman 54:17ea4b3c80de 74 return -(enc1.getAngle()-0.5372);
calamaridudeman 43:68faf056ed5c 75 }
calamaridudeman 43:68faf056ed5c 76
calamaridudeman 43:68faf056ed5c 77 void Kangaroo::setPoint(Point p){
calamaridudeman 43:68faf056ed5c 78
calamaridudeman 43:68faf056ed5c 79 Joints motors = invKinBody(p);
calamaridudeman 43:68faf056ed5c 80 m1.setPos(motors.t1);
calamaridudeman 43:68faf056ed5c 81 m2.setPos(motors.t2);
calamaridudeman 43:68faf056ed5c 82
calamaridudeman 43:68faf056ed5c 83 }
calamaridudeman 43:68faf056ed5c 84
calamaridudeman 43:68faf056ed5c 85 void Kangaroo::flightPath(){
calamaridudeman 43:68faf056ed5c 86
calamaridudeman 43:68faf056ed5c 87 }
calamaridudeman 43:68faf056ed5c 88
calamaridudeman 43:68faf056ed5c 89 void Kangaroo::start(){
calamaridudeman 43:68faf056ed5c 90 m1.start();
calamaridudeman 43:68faf056ed5c 91 m2.start();
calamaridudeman 43:68faf056ed5c 92 }