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@49:3aaa790800ad, 2013-12-01 (annotated)

Committer:
sherryxy
Date:
Sun Dec 01 23:43:58 2013 +0000
Revision:
49:3aaa790800ad
Parent:
48:8f0e007bd305
Child:
50:510ce69c2a10
trying to get calibration working

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 //m1.setVel(-.2);//wait for the legs to go to stopping points
calamaridudeman 43:68faf056ed5c 14 //m2.setVel(-.2);
calamaridudeman 43:68faf056ed5c 15
sherryxy 48:8f0e007bd305 16 m1.setPos(3.1415/5);
sherryxy 48:8f0e007bd305 17 m2.setPos(3.1415/5);//bring legs to hard stop
calamaridudeman 43:68faf056ed5c 18
calamaridudeman 37:bf257a0154db 19 led4=1;
calamaridudeman 43:68faf056ed5c 20 wait(3.0);
calamaridudeman 37:bf257a0154db 21
calamaridudeman 37:bf257a0154db 22 m1.zero();
calamaridudeman 37:bf257a0154db 23 m2.zero();
sherryxy 49:3aaa790800ad 24 m1.calibAngle(0);
sherryxy 49:3aaa790800ad 25 m2.calibAngle(2*3.1415/3);
calamaridudeman 43:68faf056ed5c 26
sherryxy 49:3aaa790800ad 27 wait(1);
sherryxy 48:8f0e007bd305 28
sherryxy 49:3aaa790800ad 29 m1.setPos(0);
sherryxy 49:3aaa790800ad 30 m2.setPos(3.1415/2);
calamaridudeman 43:68faf056ed5c 31
calamaridudeman 37:bf257a0154db 32 enc1.reset();
calamaridudeman 37:bf257a0154db 33 enc2.reset();
calamaridudeman 37:bf257a0154db 34 led2=1;
calamaridudeman 37:bf257a0154db 35 while(((enc1.getRevolutions()==0)||(enc2.getRevolutions()==0))){
calamaridudeman 37:bf257a0154db 36 wait(0.1);
calamaridudeman 37:bf257a0154db 37 }
calamaridudeman 37:bf257a0154db 38
calamaridudeman 37:bf257a0154db 39 led1=1;
calamaridudeman 43:68faf056ed5c 40
sherryxy 49:3aaa790800ad 41 //setPoint(Point(0,-.08,0));
sherryxy 48:8f0e007bd305 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 49:3aaa790800ad 53 pc.printf("x: %f y: %f\n", getPoint().x, getPoint().y);
calamaridudeman 37:bf257a0154db 54 //pc.printf("motor1: %i\n",m1.getPos());
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 43:68faf056ed5c 68 float Kangaroo::getAngle(){
calamaridudeman 43:68faf056ed5c 69 return (enc2.getAngle())+1.770973;
calamaridudeman 43:68faf056ed5c 70 }
calamaridudeman 43:68faf056ed5c 71
calamaridudeman 43:68faf056ed5c 72 void Kangaroo::setPoint(Point p){
calamaridudeman 43:68faf056ed5c 73
calamaridudeman 43:68faf056ed5c 74 Joints motors = invKinBody(p);
calamaridudeman 43:68faf056ed5c 75 m1.setPos(motors.t1);
calamaridudeman 43:68faf056ed5c 76 m2.setPos(motors.t2);
calamaridudeman 43:68faf056ed5c 77
calamaridudeman 43:68faf056ed5c 78 }
calamaridudeman 43:68faf056ed5c 79
calamaridudeman 43:68faf056ed5c 80 void Kangaroo::flightPath(){
calamaridudeman 43:68faf056ed5c 81
calamaridudeman 43:68faf056ed5c 82 }
calamaridudeman 43:68faf056ed5c 83
calamaridudeman 43:68faf056ed5c 84 void Kangaroo::start(){
calamaridudeman 43:68faf056ed5c 85 m1.start();
calamaridudeman 43:68faf056ed5c 86 m2.start();
calamaridudeman 43:68faf056ed5c 87 }