Diff: main.cpp

Revision:

41:b5a2fbc20beb

Parent:

39:309f38d1e49c

Child:

43:d5aaff7d2bec

--- a/main.cpp	Tue Apr 05 15:02:12 2016 +0000
+++ b/main.cpp	Wed Apr 13 11:27:34 2016 +0000
@@ -1,137 +1,99 @@
-#include "Odometry/Odometry.h"
-#include "Map/Map.h"
+#include "func.h"
 
 #define dt 10000
 #define ATTENTE 0
 #define GO 1
 #define STOP 2
 
+/* Déclaration des différents éléments de l'IHM */
 InterruptIn mybutton(USER_BUTTON);
 DigitalIn button(USER_BUTTON);
 DigitalOut led(LED1);
+DigitalOut bleu(PC_5);
+DigitalOut blanc(PC_6);
+DigitalOut rouge(PC_8);
+
+/* AX12 */
+AX12 left_hand(PA_15, PB_7, 4, 250000);
+AX12 right_hand(PA_15, PB_7, 1, 250000);
+
+/* Sharp */
+AnalogIn capt1(PA_4);
+AnalogIn capt2(PB_0);
+AnalogIn capt3(PC_1);
+AnalogIn capt4(PC_0);
+
+/* Moteurs pas à pas */
+Stepper RMot(NC, PB_10, PA_8);
+Stepper ZMot(NC, PB_5, PB_4);
+Stepper LMot(NC, PB_3, PA_10);
+/* Fins de course */
+InterruptIn EndR(PB_15);
+InterruptIn EndZ(PB_14);
+InterruptIn EndL(PB_13);
+
 Ticker ticker;
-//Serial pc(USBTX, USBRX);
-Serial logger (PA_9, PA_10);
+
+Serial logger(USBTX, USBRX);
+//Serial logger(PA_9, PA_10);
 RoboClaw roboclaw(460800, PA_11, PA_12);
 Odometry odo(61.7, 61.8, ENTRAXE, 4096, roboclaw);
 
-int i = 0;
+int i = 0, state = 0;
+bool EL = false, EZ = false, ER = false;
 
 void update_main(void);
 void init(void);
-void pressed(void);
-void unpressed(void);
 
 /** Debut du programme */
 int main(void)
 {
     init();
     /* Code AStar */
-    
-    double angle_v = 2*PI/5;
+
+    /*double angle_v = 2*PI/5;
     double distance_v = 200.0;
     std::vector<SimplePoint> path;
     Map map;
-    
-    
+
+
     //Construction des obstacles
     map.build();
-    
+
     float x=odo.getX();
     float y=odo.getY();
     float the = 0;
-    
+
     map.AStar(x, y, 1600, 1000, 75);
     path = map.path;
-    
+
     for(int i=0; i<path.size();i++) {
-        the = (float) atan2((double) (path[i].y - odo.getY()), (double) (path[i].x - odo.getX())); 
-        odo.GotoXYT(path[i].x, path[i].y, the);
-    }
-    
-    map.AStar(odo.getX(), odo.getY(), 0, 1000, 75);
-    path = map.path;
-    
-    for(int i=0; i<path.size();i++) {
-        the = (float) atan2((double) (path[i].y - odo.getY()), (double) (path[i].x - odo.getX())); 
+        the = (float) atan2((double) (path[i].y - odo.getY()), (double) (path[i].x - odo.getX()));
         odo.GotoXYT(path[i].x, path[i].y, the);
     }
-    
+
+    map.AStar(odo.getX(), odo.getY(), 0, 1000, 75);
+    path = map.path;
+
+    for(int i=0; i<path.size();i++) {
+        the = (float) atan2((double) (path[i].y - odo.getY()), (double) (path[i].x - odo.getX()));
+        odo.GotoXYT(path[i].x, path[i].y, the);
+    }*/
+
     //odo.GotoThet(PI);
-    odo.GotoThet(0);
+    //odo.GotoThet(0);
     //odo.TestEntraxe(3);
-    
+
     //odo.GotoThet(-PI/2);
-    wait(2000);
+    //wait(2000);
     //odo.GotoXYT(2250,500,0);
-    
+
     //odo.TestEntraxe(5);
     //odo.Forward(1000);
-    
-    /* Code JPO :
-    roboclaw.ForwardM1(ADR, 0);
-    roboclaw.ForwardM2(ADR, 0);
-    int state = 0;
     while(1)
     {
-       // while(logger.readable()) 
-        //{
-            char c = logger.getc();
-            if(c=='z')
-            {
-                if (state != 1) {
-                    state = 1;
-                    logger.printf("Avant (Z) \r\n");
-                    roboclaw.SpeedAccelM1(ADR, accel_angle, vitesse_angle);
-                    roboclaw.SpeedAccelM2(ADR, accel_angle, vitesse_angle);
-                }
-            }
-            else if(c == 's')
-            {
-                if (state != 2) {
-                    state = 2;
-                    logger.printf("Stop (S) \r\n");
-                    roboclaw.ForwardM1(ADR, 0);
-                    roboclaw.ForwardM2(ADR, 0);
-                }
-            }
-            else if(c == 'd')
-            {
-                if (state != 3) {
-                    state = 3;
-                    logger.printf("Droite (D) \r\n");
-                    roboclaw.SpeedAccelM1(ADR, accel_angle, vitesse_angle);
-                    roboclaw.SpeedAccelM2(ADR, accel_angle, -vitesse_angle);
-                }
-            }
-            else if(c == 'q')
-            {
-                if (state != 4) {
-                    state = 4;
-                    logger.printf("Gauche (Q)\r\n");
-                    roboclaw.SpeedAccelM1(ADR, accel_angle, -vitesse_angle);
-                    roboclaw.SpeedAccelM2(ADR, accel_angle, vitesse_angle);
-                }
-            }
-            else if(c == 'x')
-            {
-                if (state != 5) {
-                    state = 5;
-                    roboclaw.SpeedAccelM1M2(ADR, accel_angle, -vitesse_angle, accel_angle, -vitesse_angle);
-                }
-            }
-            else if (c == '\0') { ; }
-            else {
-                if (state != 0) {
-                    roboclaw.SpeedAccelM1M2(ADR, accel_angle, 0, accel_angle, 0);
-                    state = 0;
-                }
-            }
-       // }
-       // roboclaw.ForwardM1(ADR, 0);
-       // roboclaw.ForwardM2(ADR, 0);
-       
-    }*/
+        JPO();
+    }
 }
 
 void init(void)
@@ -141,38 +103,35 @@
     logger.baud(9600);
     logger.printf("Hello from main !\n\r");
     wait_ms(500);
+    bleu = 1;
+    blanc = 1;
+    rouge = 1;
+    wait_ms(1000);
+    bleu = 0;
+    blanc = 0;
+    rouge = 0;
     
-    odo.setPos(0, 1000, 0);
-        
+    //odo.setPos(0, 1000, 0);
     while(button);
     wait(1);
+
     mybutton.fall(&pressed);
     mybutton.rise(&unpressed);
+
+    EndL.fall(&ELpressed);
+    EndL.rise(&ELunpressed);
+    EndZ.fall(&EZpressed);
+    EndZ.rise(&EZunpressed);
+    EndR.fall(&ERpressed);
+    EndR.rise(&ERunpressed);
+
+    wait_ms(100);
     ticker.attach_us(&update_main,dt); // 100 Hz
-    
     logger.printf("End init\n\r");
 }
 
 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);
-    //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--;
-    }
+    checkAround();
 }
\ No newline at end of file