ARES / Mbed 2 deprecated TestMyPathFind

Test du path finding

Dependencies:   RoboClaw mbed

Fork of TestMyPathFind by Romain Ame

main.cpp.orig@26:6c5c453602ff, 2016-01-20 (annotated)

Committer:
IceTeam
Date:
Wed Jan 20 17:14:29 2016 +0100
Revision:
26:6c5c453602ff
Parent:
17:e32b4b54fc04 
Fusion

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IceTeam 17:e32b4b54fc04 1 #include "Odometry.h"
IceTeam 26:6c5c453602ff 2 /** Dependencies :
IceTeam 26:6c5c453602ff 3 Map/Point.h
IceTeam 26:6c5c453602ff 4 Map/define.h
IceTeam 26:6c5c453602ff 5 Map/Obstacles/Obstacle.h
IceTeam 26:6c5c453602ff 6
IceTeam 26:6c5c453602ff 7 Test en cours :
IceTeam 26:6c5c453602ff 8 * Un obstacle seulement
IceTeam 26:6c5c453602ff 9 * Radius du robot réduit à 1 (defines.cpp)
IceTeam 26:6c5c453602ff 10 * Nombreux
IceTeam 26:6c5c453602ff 11 */
IceTeam 26:6c5c453602ff 12 #include "Map/Map.h"
IceTeam 17:e32b4b54fc04 13
IceTeam 17:e32b4b54fc04 14 #define dt 10000
IceTeam 17:e32b4b54fc04 15 #define ATTENTE 0
IceTeam 17:e32b4b54fc04 16 #define GO 1
IceTeam 17:e32b4b54fc04 17 #define STOP 2
IceTeam 17:e32b4b54fc04 18
IceTeam 17:e32b4b54fc04 19 InterruptIn mybutton(USER_BUTTON);
IceTeam 17:e32b4b54fc04 20 DigitalIn button(USER_BUTTON);
IceTeam 17:e32b4b54fc04 21 DigitalOut led(LED1);
IceTeam 17:e32b4b54fc04 22 Ticker ticker;
IceTeam 17:e32b4b54fc04 23 //Serial pc(USBTX, USBRX);
IceTeam 17:e32b4b54fc04 24 Serial pc(PA_9, PA_10);
IceTeam 17:e32b4b54fc04 25 RoboClaw roboclaw(460800, PA_11, PA_12);
IceTeam 17:e32b4b54fc04 26 Odometry odo(63.2, 63.3, 252, 4096, roboclaw);
IceTeam 17:e32b4b54fc04 27
IceTeam 17:e32b4b54fc04 28 int i = 0;
IceTeam 17:e32b4b54fc04 29
IceTeam 17:e32b4b54fc04 30 void update_main(void);
IceTeam 17:e32b4b54fc04 31 void init(void);
IceTeam 17:e32b4b54fc04 32 void pressed(void);
IceTeam 17:e32b4b54fc04 33 void unpressed(void);
IceTeam 17:e32b4b54fc04 34
IceTeam 17:e32b4b54fc04 35 /** Debut du programme */
IceTeam 17:e32b4b54fc04 36 int main(void)
IceTeam 17:e32b4b54fc04 37 {
IceTeam 26:6c5c453602ff 38 double angle_v = 2*PI/5;
IceTeam 26:6c5c453602ff 39 double distance_v = 200.0;
IceTeam 26:6c5c453602ff 40 std::vector<SimplePoint> path;
IceTeam 26:6c5c453602ff 41 Map map;
IceTeam 26:6c5c453602ff 42
IceTeam 17:e32b4b54fc04 43 init();
IceTeam 26:6c5c453602ff 44 //Construction des obstacles
IceTeam 26:6c5c453602ff 45 map.build();
IceTeam 26:6c5c453602ff 46 map.Astar(0, 1000, 2000, 1000);
IceTeam 26:6c5c453602ff 47 path = map.path;
IceTeam 26:6c5c453602ff 48
IceTeam 26:6c5c453602ff 49 for(int i=0; i<path.size();i++) {
IceTeam 26:6c5c453602ff 50 odo.GotoXYT(path[i].x, path[i].y, 0);
IceTeam 26:6c5c453602ff 51 }
IceTeam 26:6c5c453602ff 52
IceTeam 26:6c5c453602ff 53 //odo.GotoXYT(500, 50, 0);
IceTeam 26:6c5c453602ff 54 //odo.GotoXYT(200, 0, 0);
IceTeam 26:6c5c453602ff 55 //odo.GotoXYT(0, 0, 0);
IceTeam 26:6c5c453602ff 56
IceTeam 26:6c5c453602ff 57 //odo.GotoThet(-PI/2);
IceTeam 17:e32b4b54fc04 58 wait(2000);
IceTeam 26:6c5c453602ff 59 //odo.GotoXYT(2250,500,0);
IceTeam 17:e32b4b54fc04 60 while(1);
IceTeam 17:e32b4b54fc04 61 }
IceTeam 17:e32b4b54fc04 62
IceTeam 17:e32b4b54fc04 63 void init(void)
IceTeam 17:e32b4b54fc04 64 {
IceTeam 17:e32b4b54fc04 65 pc.baud(9600);
IceTeam 17:e32b4b54fc04 66 pc.printf("Hello from main !\n\r");
IceTeam 17:e32b4b54fc04 67 wait_ms(500);
IceTeam 17:e32b4b54fc04 68
IceTeam 26:6c5c453602ff 69 odo.setPos(0, 0, 0);
IceTeam 17:e32b4b54fc04 70 roboclaw.ForwardM1(ADR, 0);
IceTeam 17:e32b4b54fc04 71 roboclaw.ForwardM2(ADR, 0);
IceTeam 17:e32b4b54fc04 72
IceTeam 17:e32b4b54fc04 73 while(button);
IceTeam 17:e32b4b54fc04 74 wait(1);
IceTeam 17:e32b4b54fc04 75 mybutton.fall(&pressed);
IceTeam 17:e32b4b54fc04 76 mybutton.rise(&unpressed);
IceTeam 17:e32b4b54fc04 77 ticker.attach_us(&update_main,dt); // 100 Hz
IceTeam 17:e32b4b54fc04 78 }
IceTeam 17:e32b4b54fc04 79
IceTeam 17:e32b4b54fc04 80 void update_main(void)
IceTeam 17:e32b4b54fc04 81 {
IceTeam 17:e32b4b54fc04 82 odo.update_odo();
IceTeam 17:e32b4b54fc04 83 //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", odo.getX(), odo.getY(), odo.getTheta()*180/PI);
IceTeam 17:e32b4b54fc04 84 //if(pc.readable()) if(pc.getc()=='l') led = !led;
IceTeam 17:e32b4b54fc04 85 }
IceTeam 17:e32b4b54fc04 86
IceTeam 17:e32b4b54fc04 87 void pressed(void)
IceTeam 17:e32b4b54fc04 88 {
IceTeam 17:e32b4b54fc04 89 if(i==0) {
IceTeam 17:e32b4b54fc04 90 roboclaw.ForwardM1(ADR, 0);
IceTeam 17:e32b4b54fc04 91 roboclaw.ForwardM2(ADR, 0);
IceTeam 17:e32b4b54fc04 92 //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", odo.getX(), odo.getY(), odo.getTheta()*180/PI);
IceTeam 17:e32b4b54fc04 93 i++;
IceTeam 17:e32b4b54fc04 94 }
IceTeam 17:e32b4b54fc04 95 }
IceTeam 17:e32b4b54fc04 96
IceTeam 17:e32b4b54fc04 97 void unpressed(void)
IceTeam 17:e32b4b54fc04 98 {
IceTeam 17:e32b4b54fc04 99 if(i==1) {
IceTeam 17:e32b4b54fc04 100 i--;
IceTeam 17:e32b4b54fc04 101 }
IceTeam 17:e32b4b54fc04 102 }