Revision 17:e32b4b54fc04, committed 2016-01-07

Comitter:

IceTeam

Date:

Thu Jan 07 14:08:33 2016 +0100

Parent:

16:a1ede21a963b

Child:

18:0f1fefe78266

Commit message:

Test AStar

Changed in this revision

--- a/main.cpp	Thu Jan 07 13:56:38 2016 +0100
+++ b/main.cpp	Thu Jan 07 14:08:33 2016 +0100
@@ -1,4 +1,9 @@
 #include "Odometry.h"
+/** Dependencies :
+    Map/Point.h
+    Map/define.h
+    Map/Obstacles/Obstacle.h
+*/ 
 #include "Map/Map.h"
 
 #define dt 10000
@@ -30,9 +35,9 @@
     std::vector<SimplePoint> path;
     Map map;
     
-    map.build();
     init();
 
+    map.build();
     map.Astar(0, 1000, 2000, 1000, 1);
     path = map.path;
     

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp.orig	Thu Jan 07 14:08:33 2016 +0100
@@ -0,0 +1,81 @@
+#include "Odometry.h"
+
+#define dt 10000
+#define ATTENTE 0
+#define GO 1
+#define STOP 2
+
+InterruptIn mybutton(USER_BUTTON);
+DigitalIn button(USER_BUTTON);
+DigitalOut led(LED1);
+Ticker ticker;
+//Serial pc(USBTX, USBRX);
+Serial pc(PA_9, PA_10);
+RoboClaw roboclaw(460800, PA_11, PA_12);
+Odometry odo(63.2, 63.3, 252, 4096, roboclaw);
+
+int i = 0;
+
+void update_main(void);
+void init(void);
+void pressed(void);
+void unpressed(void);
+
+/** Debut du programme */
+int main(void)
+{
+    init();
+    //roboclaw.SpeedAccelDeccelPositionM1M2(ADR, accel_angle, vitesse_angle, deccel_angle, -116878, accel_angle, vitesse_angle, deccel_angle, 116878, 1);
+    odo.GotoXY(1800,1500);
+    odo.GotoXY(2500,500);
+    odo.GotoXY(2000,300);
+    odo.GotoXYT(300,1000,0);
+    //for(int n=0; n<40; n++) odo.setTheta();
+    odo.GotoThet(-PI);
+    odo.GotoThet(0);
+    wait(2000);
+    while(1);
+}
+
+void init(void)
+{
+    pc.baud(9600);
+    pc.printf("Hello from main !\n\r");
+    wait_ms(500);
+    
+    odo.setPos(300, 1000, 0);
+    roboclaw.ForwardM1(ADR, 0);
+    roboclaw.ForwardM2(ADR, 0);
+    
+    while(button);
+    wait(1);
+    mybutton.fall(&pressed);
+    mybutton.rise(&unpressed);
+    ticker.attach_us(&update_main,dt); // 100 Hz
+}
+
+void update_main(void)
+{
+    odo.update_odo();
+    //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", odo.getX(), odo.getY(), odo.getTheta()*180/PI);
+    //pc.printf("Theta : %3.2f\n\r", odo.getTheta()*180/PI);
+    pc.printf("X : %4.2f\n\r", odo.getX());
+    //if(pc.readable()) if(pc.getc()=='l') led = !led;
+}
+
+void pressed(void)
+{
+    if(i==0) {
+        roboclaw.ForwardM1(ADR, 0);
+        roboclaw.ForwardM2(ADR, 0);
+        //pc.printf("X : %3.2f\tY : %3.2f\tTheta : %3.2f\n\r", odo.getX(), odo.getY(), odo.getTheta()*180/PI);
+        i++;
+    }
+}
+
+void unpressed(void)
+{
+    if(i==1) {
+        i--;
+    }
+}
\ No newline at end of file