ARES / Mbed 2 deprecated TestMyPathFind

Test du path finding

Dependencies:   RoboClaw mbed

Fork of TestMyPathFind by Romain Ame

main.cpp@41:53d5990ff99d, 2016-04-13 (annotated)

Committer:
IceTeam
Date:
Wed Apr 13 16:17:19 2016 +0000
Revision:
41:53d5990ff99d
Parent:
39:ca4dd3faffa8 
Correction odometrie;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IceTeam 31:8bcc3a0bfa8a 1 #include "Odometry/Odometry.h"
IceTeam 39:ca4dd3faffa8 2 #include "map/map.h"
sype 0:ad9600df4a70 3
sype 0:ad9600df4a70 4 #define dt 10000
sype 10:ae3178aa94e9 5 #define ATTENTE 0
sype 10:ae3178aa94e9 6 #define GO 1
sype 10:ae3178aa94e9 7 #define STOP 2
IceTeam 36:2d7357a385bc 8 #define ENTRAXE 248.25
sype 0:ad9600df4a70 9
sype 10:ae3178aa94e9 10 InterruptIn mybutton(USER_BUTTON);
sype 10:ae3178aa94e9 11 DigitalIn button(USER_BUTTON);
sype 2:abdf8c6823a1 12 DigitalOut led(LED1);
sype 0:ad9600df4a70 13 Ticker ticker;
sype 10:ae3178aa94e9 14 //Serial pc(USBTX, USBRX);
IceTeam 21:077bc7634b90 15 Serial logger (PA_9, PA_10);
sype 10:ae3178aa94e9 16 RoboClaw roboclaw(460800, PA_11, PA_12);
IceTeam 36:2d7357a385bc 17 Odometry odo(63.2, 63.3, ENTRAXE, 4096, roboclaw);
sype 10:ae3178aa94e9 18
sype 10:ae3178aa94e9 19 int i = 0;
sype 0:ad9600df4a70 20
sype 0:ad9600df4a70 21 void update_main(void);
sype 2:abdf8c6823a1 22 void init(void);
sype 10:ae3178aa94e9 23 void pressed(void);
sype 10:ae3178aa94e9 24 void unpressed(void);
sype 0:ad9600df4a70 25
IceTeam 9:e39b218ab20d 26 /** Debut du programme */
sype 0:ad9600df4a70 27 int main(void)
sype 10:ae3178aa94e9 28 {
sype 38:da3a2c781672 29 init();
IceTeam 41:53d5990ff99d 30
IceTeam 39:ca4dd3faffa8 31 double angle_v = 2*PI/5;
sype 10:ae3178aa94e9 32 double distance_v = 200.0;
IceTeam 39:ca4dd3faffa8 33 float the;
sype 38:da3a2c781672 34
IceTeam 41:53d5990ff99d 35 logger.printf("Yeah !\n\r");
IceTeam 41:53d5990ff99d 36
IceTeam 41:53d5990ff99d 37 roboclaw.ForwardM1(ADR, 0);
IceTeam 41:53d5990ff99d 38 roboclaw.ForwardM2(ADR, 0);
IceTeam 41:53d5990ff99d 39
IceTeam 41:53d5990ff99d 40 odo.setPos(110, 1000, 0);
IceTeam 41:53d5990ff99d 41
IceTeam 41:53d5990ff99d 42 odo.GotoXY(1500,1500);
IceTeam 41:53d5990ff99d 43 odo.GotoXY(1500,500);
IceTeam 41:53d5990ff99d 44 odo.GotoXY(1000,1500);
IceTeam 41:53d5990ff99d 45 odo.GotoXY(500,1000);
IceTeam 41:53d5990ff99d 46
IceTeam 41:53d5990ff99d 47 roboclaw.ForwardM1(ADR, 0);
IceTeam 41:53d5990ff99d 48 roboclaw.ForwardM2(ADR, 0);
IceTeam 41:53d5990ff99d 49
IceTeam 41:53d5990ff99d 50 /*
IceTeam 39:ca4dd3faffa8 51 point dep (odo.getX(), odo.getY());
IceTeam 41:53d5990ff99d 52 point arr (1500, 1000);
IceTeam 28:142ebc5410f3 53
IceTeam 39:ca4dd3faffa8 54 map m;
IceTeam 41:53d5990ff99d 55 m.addObs(obsCarr (550, 600, 50, 600));
IceTeam 39:ca4dd3faffa8 56 m.addObs(obsCarr (1170, 1260, 220, 220));
IceTeam 41:53d5990ff99d 57
IceTeam 41:53d5990ff99d 58 roboclaw.ForwardM1(ADR, 0);
IceTeam 41:53d5990ff99d 59 roboclaw.ForwardM2(ADR, 0);
IceTeam 30:58bfac39e701 60
IceTeam 39:ca4dd3faffa8 61 m.FindWay (dep, arr);
IceTeam 39:ca4dd3faffa8 62 logger.printf("On a cherche un chemin\n\r");
IceTeam 13:a394e27b8cb2 63
IceTeam 39:ca4dd3faffa8 64 if (m.getEnded()) {
IceTeam 39:ca4dd3faffa8 65 nVector<pointParcours> p = m.getParc ();
IceTeam 39:ca4dd3faffa8 66 logger.printf ("Il y a %d points de parcours\n\r", p.size());
IceTeam 39:ca4dd3faffa8 67 for (int i = 0; i < p.size (); i++) {
IceTeam 39:ca4dd3faffa8 68 logger.printf("Goto [%f, %f]\n\r", p[i].getX(), p[i].getY());
IceTeam 41:53d5990ff99d 69 }
IceTeam 41:53d5990ff99d 70 logger.printf("\n\n\r");
IceTeam 41:53d5990ff99d 71
IceTeam 41:53d5990ff99d 72 for (int i = 0; i < p.size (); i++) {
IceTeam 41:53d5990ff99d 73 logger.printf("Goto [%f, %f]\n\r", p[i].getX(), p[i].getY());
IceTeam 39:ca4dd3faffa8 74 //the = (float) atan2((double) (p[i].gety() - odo.getY()), (double) (p[i].getx() - odo.getX()));
IceTeam 39:ca4dd3faffa8 75 odo.GotoXY(p[i].getX(), p[i].getY());
IceTeam 39:ca4dd3faffa8 76 }
IceTeam 18:0f1fefe78266 77 }
IceTeam 39:ca4dd3faffa8 78 else {
IceTeam 39:ca4dd3faffa8 79 logger.printf("Chemin pas trouve ...");
IceTeam 27:ae68960316f1 80 }
IceTeam 30:58bfac39e701 81
IceTeam 36:2d7357a385bc 82 //odo.GotoThet(PI);
IceTeam 36:2d7357a385bc 83 odo.GotoThet(0);
IceTeam 36:2d7357a385bc 84 //odo.TestEntraxe(3);
IceTeam 31:8bcc3a0bfa8a 85
sype 10:ae3178aa94e9 86 //odo.GotoThet(-PI/2);
IceTeam 41:53d5990ff99d 87
IceTeam 41:53d5990ff99d 88 roboclaw.ForwardM1(ADR, 0);
IceTeam 41:53d5990ff99d 89 roboclaw.ForwardM2(ADR, 0);
IceTeam 41:53d5990ff99d 90
sype 10:ae3178aa94e9 91 wait(2000);
IceTeam 39:ca4dd3faffa8 92 while (1);
sype 38:da3a2c781672 93 //odo.GotoXYT(2250,500,0);*/
sype 2:abdf8c6823a1 94 }
sype 2:abdf8c6823a1 95
sype 2:abdf8c6823a1 96 void init(void)
sype 0:ad9600df4a70 97 {
sype 38:da3a2c781672 98 roboclaw.ForwardM1(ADR, 0);
sype 38:da3a2c781672 99 roboclaw.ForwardM2(ADR, 0);
sype 38:da3a2c781672 100 logger.baud(9600);
sype 38:da3a2c781672 101 logger.printf("Hello from main !\n\r");
sype 10:ae3178aa94e9 102 wait_ms(500);
sype 10:ae3178aa94e9 103
IceTeam 27:ae68960316f1 104 odo.setPos(0, 1000, 0);
sype 38:da3a2c781672 105
sype 10:ae3178aa94e9 106 while(button);
sype 10:ae3178aa94e9 107 wait(1);
sype 10:ae3178aa94e9 108 mybutton.fall(&pressed);
IceTeam 39:ca4dd3faffa8 109 mybutton.rise(&unpressed);
sype 0:ad9600df4a70 110 ticker.attach_us(&update_main,dt); // 100 Hz
IceTeam 39:ca4dd3faffa8 111
IceTeam 39:ca4dd3faffa8 112 logger.printf("End init\n\r");
sype 0:ad9600df4a70 113 }
sype 0:ad9600df4a70 114
sype 0:ad9600df4a70 115 void update_main(void)
sype 0:ad9600df4a70 116 {
sype 0:ad9600df4a70 117 odo.update_odo();
sype 10:ae3178aa94e9 118 //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", odo.getX(), odo.getY(), odo.getTheta()*180/PI);
sype 2:abdf8c6823a1 119 //if(pc.readable()) if(pc.getc()=='l') led = !led;
sype 0:ad9600df4a70 120 }
sype 10:ae3178aa94e9 121
sype 10:ae3178aa94e9 122 void pressed(void)
sype 10:ae3178aa94e9 123 {
sype 10:ae3178aa94e9 124 if(i==0) {
sype 10:ae3178aa94e9 125 roboclaw.ForwardM1(ADR, 0);
sype 10:ae3178aa94e9 126 roboclaw.ForwardM2(ADR, 0);
sype 10:ae3178aa94e9 127 //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", odo.getX(), odo.getY(), odo.getTheta()*180/PI);
sype 10:ae3178aa94e9 128 i++;
sype 10:ae3178aa94e9 129 }
sype 10:ae3178aa94e9 130 }
sype 10:ae3178aa94e9 131
sype 10:ae3178aa94e9 132 void unpressed(void)
sype 10:ae3178aa94e9 133 {
sype 10:ae3178aa94e9 134 if(i==1) {
sype 10:ae3178aa94e9 135 i--;
sype 10:ae3178aa94e9 136 }
sype 10:ae3178aa94e9 137 }