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@48:8f0e007bd305, 2013-12-01 (annotated)

Committer:
sherryxy
Date:
Sun Dec 01 22:19:48 2013 +0000
Revision:
48:8f0e007bd305
Parent:
45:0db0fc9f77b1
Child:
49:3aaa790800ad
landing detection tests with kankan control

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();
calamaridudeman 37:bf257a0154db 24
calamaridudeman 43:68faf056ed5c 25 m1.setPos(0); //-3.1415/2);
calamaridudeman 43:68faf056ed5c 26 m2.setPos(-3.1415/2); //bring legs to neutral position
calamaridudeman 43:68faf056ed5c 27
sherryxy 48:8f0e007bd305 28
calamaridudeman 43:68faf056ed5c 29 m1.zero(); //rezero at neutral
calamaridudeman 43:68faf056ed5c 30 m2.zero();
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 48:8f0e007bd305 41 //setPoint(Point(0,-.1,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");
calamaridudeman 37:bf257a0154db 53
calamaridudeman 37:bf257a0154db 54 //pc.printf("motor1: %i\n",m1.getPos());
calamaridudeman 37:bf257a0154db 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",);
calamaridudeman 37:bf257a0154db 58 pc.printf("clix2: %i, rots2: %i\n",enc2.getPulses(), enc2.getRevolutions());
calamaridudeman 43:68faf056ed5c 59 }
calamaridudeman 43:68faf056ed5c 60
sherryxy 48:8f0e007bd305 61 Point Kangaroo
sherryxy 48:8f0e007bd305 62
calamaridudeman 43:68faf056ed5c 63 float Kangaroo::getAngle(){
calamaridudeman 43:68faf056ed5c 64 return (enc2.getAngle())+1.770973;
calamaridudeman 43:68faf056ed5c 65 }
calamaridudeman 43:68faf056ed5c 66
calamaridudeman 43:68faf056ed5c 67 void Kangaroo::setPoint(Point p){
calamaridudeman 43:68faf056ed5c 68
calamaridudeman 43:68faf056ed5c 69 Joints motors = invKinBody(p);
calamaridudeman 43:68faf056ed5c 70 m1.setPos(motors.t1);
calamaridudeman 43:68faf056ed5c 71 m2.setPos(motors.t2);
calamaridudeman 43:68faf056ed5c 72
calamaridudeman 43:68faf056ed5c 73 }
calamaridudeman 43:68faf056ed5c 74
calamaridudeman 43:68faf056ed5c 75 void Kangaroo::flightPath(){
calamaridudeman 43:68faf056ed5c 76
calamaridudeman 43:68faf056ed5c 77 }
calamaridudeman 43:68faf056ed5c 78
calamaridudeman 43:68faf056ed5c 79 void Kangaroo::start(){
calamaridudeman 43:68faf056ed5c 80 m1.start();
calamaridudeman 43:68faf056ed5c 81 m2.start();
calamaridudeman 43:68faf056ed5c 82 }