Robot's source code
Dependencies: mbed
Diff: main.cpp
- Revision:
- 63:fd9af0693e50
- Parent:
- 62:454cd844fe1e
- Child:
- 65:b5d7f13870be
--- a/main.cpp Thu Apr 09 17:05:06 2015 +0000 +++ b/main.cpp Thu Apr 09 17:32:36 2015 +0000 @@ -36,7 +36,7 @@ Timer t; - AX12 test(PA_9,NC,0x03,250000); + //AX12 test(PA_9,NC,0x03,250000); AnalogIn ain0(PA_0); AnalogIn ain1(PA_1); @@ -50,35 +50,25 @@ QEI qei_left(PB_4,PB_5,NC,1024*4,QEI::X4_ENCODING); Odometry odometry(&qei_left,&qei_right,63/2.,63/2.,280); - DigitalOut led(LED1); + DigitalOut led1(LED1); + DigitalOut led2(LED2); + DigitalOut led3(LED3); - test.SetMode(0); // Position - + t.start(); + t.reset(); while(1) { - test.SetGoal(0); - wait(1); - test.SetGoal(90); - wait(1); + odometry.update(t.read()); + t.reset(); + + logger.printf("---------\r\n"); + logger.printf("%f %f %f\r\n",odometry.getX(),odometry.getY(),odometry.getTheta()); + logger.printf("%f %f %f\r\n",odometry.getVitLeft(),odometry.getVitRight(),t.read()); + t.reset(); + wait(0.5); + motorL.setSpeed(1); } - - - /*char dataOff[] = {0}; - char dataOn[] = {1}; - while(1) - { - logger.printf("0: %f\r\n",ain0.read()*3.3); - logger.printf("1: %f\r\n",ain1.read()*3.3); - logger.printf("2: %f\r\n",ain2.read()*3.3); - logger.printf("3: %f\r\n",ain3.read()*3.3); - logger.printf("4: %f\r\n\r\n",ain4.read()*3.3); - led = !led; - wait(1); - motorR.setSpeed(1.0); - wait(1); - motorR.setSpeed(0.0); - }*/ bool testOdo = false; @@ -90,6 +80,7 @@ aserv_planB asserv(odometry,motorL,motorR); t.start(); + t.reset(); while(!testOdo) {