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@45:0db0fc9f77b1, 2013-12-01 (annotated)

Committer:
alexc89
Date:
Sun Dec 01 07:23:22 2013 +0000
Revision:
45:0db0fc9f77b1
Parent:
43:68faf056ed5c
Child:
48:8f0e007bd305
Pos and Velocity Control Checked.   Phase detection and control structure implemented and checked.

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
calamaridudeman 43:68faf056ed5c 16 m1.setPos(3.1415/6);
calamaridudeman 43:68faf056ed5c 17 m2.setPos(3.1415/6);//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
calamaridudeman 43:68faf056ed5c 28 m1.zero(); //rezero at neutral
calamaridudeman 43:68faf056ed5c 29 m2.zero();
calamaridudeman 43:68faf056ed5c 30
calamaridudeman 37:bf257a0154db 31 enc1.reset();
calamaridudeman 37:bf257a0154db 32 enc2.reset();
calamaridudeman 37:bf257a0154db 33 led2=1;
calamaridudeman 37:bf257a0154db 34 while(((enc1.getRevolutions()==0)||(enc2.getRevolutions()==0))){
calamaridudeman 37:bf257a0154db 35 wait(0.1);
calamaridudeman 37:bf257a0154db 36 }
calamaridudeman 37:bf257a0154db 37
calamaridudeman 37:bf257a0154db 38 led1=1;
calamaridudeman 43:68faf056ed5c 39
calamaridudeman 43:68faf056ed5c 40 setPoint(Point(1.0,-1.0,0));
calamaridudeman 43:68faf056ed5c 41
calamaridudeman 37:bf257a0154db 42 led3=1;
calamaridudeman 43:68faf056ed5c 43 m1.stop();
calamaridudeman 43:68faf056ed5c 44 m2.stop();
calamaridudeman 37:bf257a0154db 45 }
calamaridudeman 37:bf257a0154db 46
calamaridudeman 37:bf257a0154db 47 void Kangaroo::testEncoders(Serial &pc){
calamaridudeman 37:bf257a0154db 48 //pc.printf("hello world\n");
calamaridudeman 37:bf257a0154db 49
calamaridudeman 37:bf257a0154db 50 //pc.printf("motor1: %i\n",m1.getPos());
calamaridudeman 37:bf257a0154db 51 pc.printf("clix1: %i, rots1: %i\n",enc1.getPulses(), enc1.getRevolutions());
calamaridudeman 37:bf257a0154db 52
calamaridudeman 37:bf257a0154db 53 //pc.printf("motor2: %i\n",);
calamaridudeman 37:bf257a0154db 54 pc.printf("clix2: %i, rots2: %i\n",enc2.getPulses(), enc2.getRevolutions());
calamaridudeman 43:68faf056ed5c 55 }
calamaridudeman 43:68faf056ed5c 56
calamaridudeman 43:68faf056ed5c 57 float Kangaroo::getAngle(){
calamaridudeman 43:68faf056ed5c 58 return (enc2.getAngle())+1.770973;
calamaridudeman 43:68faf056ed5c 59 }
calamaridudeman 43:68faf056ed5c 60
calamaridudeman 43:68faf056ed5c 61 void Kangaroo::setPoint(Point p){
calamaridudeman 43:68faf056ed5c 62
calamaridudeman 43:68faf056ed5c 63 Joints motors = invKinBody(p);
calamaridudeman 43:68faf056ed5c 64 m1.setPos(motors.t1);
calamaridudeman 43:68faf056ed5c 65 m2.setPos(motors.t2);
calamaridudeman 43:68faf056ed5c 66
calamaridudeman 43:68faf056ed5c 67 }
calamaridudeman 43:68faf056ed5c 68
calamaridudeman 43:68faf056ed5c 69 void Kangaroo::flightPath(){
calamaridudeman 43:68faf056ed5c 70
calamaridudeman 43:68faf056ed5c 71 }
calamaridudeman 43:68faf056ed5c 72
calamaridudeman 43:68faf056ed5c 73 void Kangaroo::start(){
calamaridudeman 43:68faf056ed5c 74 m1.start();
calamaridudeman 43:68faf056ed5c 75 m2.start();
calamaridudeman 43:68faf056ed5c 76 }