Diff: main.cpp

Revision:

93:4d5664e9188a

Parent:

92:b403f2724252

Child:

98:2ec4e17dfc92

Child:

100:a827a645d6c2

Child:

105:6da275b01395

--- a/main.cpp	Wed Apr 29 16:13:53 2015 +0000
+++ b/main.cpp	Thu Apr 30 15:55:09 2015 +0000
@@ -2,7 +2,7 @@
 
 #define PLAN_A
 
-//#define OUT_USB
+#define OUT_USB
 #include "defines.h"
 
 #include "QEI.h"
@@ -18,95 +18,171 @@
 #endif
 
 #include "Motor.h"
-/*----------------------------------------------------------------------------------------------*/
-/*Serial*/    
+
+void update();
+
+// *--------------------------* //
+//         Déclarations         //
+
+//        Decl. logger          //
+
+Serial logger(OUT_TX, OUT_RX); // tx, rx
+
+//        Decl. timer           //
+
+Timer t;
+Ticker ticker;
+
+//         Decl. AX12           //
+
+AX12 ax12_pince(AX12_TX,AX12_RX,5,250000);
+AX12 ax12_brasG(AX12_TX,AX12_RX,2,250000);
+AX12 ax12_brasD(AX12_TX,AX12_RX,3,250000);
+
+//        Decl. Moteurs         //
+
+PwmOut pw1(PWM_MOT1);
+DigitalOut dir1(DIR_MOT1);
+PwmOut pw2(PWM_MOT2);
+DigitalOut dir2(DIR_MOT2);
+
+Motor motorL(PWM_MOT1,DIR_MOT1);
+Motor motorR(PWM_MOT2,DIR_MOT2);
+
+//         Decl. Sharps         //
 
-//Serial logger(OUT_TX, OUT_RX); // tx, rx
-Serial logger(USBTX,USBRX);
-//logger.baud((int)115200);
-/*----------------------------------------------------------------------------------------------*/
+AnalogIn sharp_devant(SHARP_D);
+AnalogIn sharp_devant_droite(SHARP_DD);
+AnalogIn sharp_devant_gauche(SHARP_DG);
+AnalogIn sharp_arriere_gauche(SHARP_AG);
+AnalogIn sharp_arriere_droite(SHARP_AD);
+
+//        Decl. Boutons         //
+
+DigitalIn bp(BP_DESSUS);
+DigitalIn tirette(TIRETTE_DESSUS);
+DigitalIn couleur(COULEUR_DESSUS);
+
+//       Decl. Odometrie        //
 
-/* --- Initialisation de la liste des obstable --- */
-int Obstacle::lastId = 0;
+#ifdef PLAN_A
+    QEI qei_right(PB_3,PA_10,NC,1024*4,QEI::X4_ENCODING);
+    QEI qei_left(PB_4,PB_5,NC,1024*4,QEI::X4_ENCODING);
+    
+    Odometry odometry(&qei_left,&qei_right,63/2.,63/2.,255);
+#else
+    QEI qei_left(ODO_G_A,ODO_G_B,NC,1024*4,QEI::X4_ENCODING);
+    QEI qei_right(ODO_D_A,ODO_D_B,NC,1024*4,QEI::X4_ENCODING);
+    
+    Odometry2 odometry(&qei_left,&qei_right,63/2.,63/2.,255);
+#endif
+
+//         Decl. Asserv         //
+
+#ifdef PLAN_A
+    Asserv<float> instanceAsserv(&odometry);
+#else
+    aserv_planB asserv(odometry,motorL,motorR);
+#endif
+
+//           Fin Decl.          //
+// *--------------------------* //
+
 
 int main()
 {
-    logger.printf("Initialisation...\r\n");
-    
-    AX12 test(AX12_TX,AX12_RX,5,250000);
-    test.setMode(0);
+    #ifdef OUT_USB
+        logger.baud((int)921600);
+    #endif
     
-    PwmOut pw1(PWM_MOT1);
-    DigitalOut dir1(DIR_MOT1);
-    PwmOut pw2(PWM_MOT2);
-    DigitalOut dir2(DIR_MOT2);
+    // *--------------------------* //
+    //             Init             //
     
-        
-    Motor motorL(PWM_MOT1,DIR_MOT1);
-    Motor motorR(PWM_MOT2,DIR_MOT2);    
+    logger.printf("Initialisation.............");
     
-    Timer t;
+    //         Init. AX12           //
     
-    //AX12 test(PA_9,NC,0x03,250000);
+    ax12_pince.setMode(0);
+    ax12_brasG.setMode(0);
+    ax12_brasD.setMode(0);
     
-    AnalogIn ain0(PA_0);
-    AnalogIn ain1(PA_1);
-    AnalogIn ain2(PA_4);
-    AnalogIn ain3(PB_0);
-    AnalogIn ain4(PC_1);
+    //        Init. Moteurs         //
     
-    /*----------------------------------------------------------------------------------------------*/
-    /*Odometry*/
+    motorL.setSpeed(0);
+    motorR.setSpeed(0);
+    
+    //        Init. Sharps          //
+    
+    //        Init. Boutons         //
+    
+    //       Init. Odometrie        //
+    
     #ifdef PLAN_A
-        QEI qei_right(PB_3,PA_10,NC,1024*4,QEI::X4_ENCODING);
-        QEI qei_left(PB_4,PB_5,NC,1024*4,QEI::X4_ENCODING);
-        
-        Odometry odometry(&qei_left,&qei_right,63/2.,63/2.,255);
         odometry.setTheta(0);
         odometry.setX(0);
         odometry.setY(0);
     #else
-        QEI qei_left(ODO_G_A,ODO_G_B,NC,1024*4,QEI::X4_ENCODING);
-        QEI qei_right(ODO_D_A,ODO_D_B,NC,1024*4,QEI::X4_ENCODING);
-        
-        Odometry2 odometry(&qei_left,&qei_right,63/2.,63/2.,255);
-        
         odometry.setTheta(PI/2);
         odometry.setX(0);
         odometry.setY(0);
     #endif
     
-    DigitalOut led1(LED1);
-    DigitalOut led2(LED2);
-    DigitalOut led3(LED3);
+    logger.printf("[done]\r\n");
+    
+    //           Fin Init.          //
+    // *--------------------------* //
+    
+    // *--------------------------* //
+    //              MIP             //
+    
+    logger.printf("Appuyer sur le bouton pour mettre ne position\r\n");
+    
+    while(!bp); // On attend le top de mise en position
+    
+    logger.printf("MIP........................");
+    
+    ax12_pince.setMaxTorque(MAX_TORQUE);
+    ax12_brasG.setMaxTorque(MAX_TORQUE);
+    ax12_brasD.setMaxTorque(MAX_TORQUE);
     
-    bool testOdo = false;
+    ax12_pince.setGoal(PINCE_FERMEE);
+    ax12_brasG.setGoal(BRASG_OUVERT);
+    ax12_brasD.setGoal(BRASD_OUVERT);
+    wait(1.5);
+    ax12_pince.setGoal(PINCE_OUVERTE);
+    ax12_brasG.setGoal(BRASG_FERME);
+    ax12_brasD.setGoal(BRASD_FERME);
+    wait(1.5);
+    ax12_pince.setMaxTorque(0);
+    ax12_brasG.setMaxTorque(0);
+    ax12_brasD.setMaxTorque(0);
+    
+    logger.printf("[done]\r\n");
+    
+    //                              //
+    // *--------------------------* //
+    
+    // *--------------------------* //
+    //            Asserv            //
+    
+    logger.printf("Demarrage asserv...........");
+    t.start();
+    ticker.attach_us(&update,10000); //100Hz
+    logger.printf("[done]\r\n");
+    
+    //                              //
+    // *--------------------------* //
+
+    bool continuer = true;
     
     #ifdef PLAN_A
-        Asserv<float> instanceAsserv(&odometry);
-        //instanceAsserv.setGoal((float)0.0,(float)0.0,(float)0);
         instanceAsserv.setGoal(300.0f,200.0f,0.0f);
         logger.printf("GOAL SET... RUNNING!\r\n");
         
         char state = 0;
         
-        t.start();
-        t.reset();
-        
-        while(!testOdo)
+        while(continuer)
         {
-            float dt = t.read();
-            t.reset();
-            
-            odometry.update(dt);
-            instanceAsserv.update(dt);
-            float phi_r = (float)instanceAsserv.getPhiR();
-            float phi_l = (float)instanceAsserv.getPhiL();
-            float phi_max = (float)instanceAsserv.getPhiMax();
-                        
-            
-            motorR.setSpeed(0.08+(phi_r/phi_max));
-            motorL.setSpeed(0.08+(phi_l/phi_max));
             #define test
             #ifdef test            
                 if(state == 0 && instanceAsserv.isArrivedRho())
@@ -133,29 +209,27 @@
             #endif
         }
     #else
-        aserv_planB asserv(odometry,motorL,motorR);
         asserv.setGoal(45,45,0);
-        
-        t.start();
-        t.reset();
-        
-        while(!testOdo)
-        {
-            //Parametrage des coef par bluetooth
-            /*while(logger.readable())
-            {
-                char c = logger.getc();
-                if(c=='a') asserv.Kd += 0.001;
-                else if(c=='z') asserv.Kd -= 0.001;
-                logger.printf("%f\n\r",asserv.Kd);
-            }*/
-            
-            //Asservissement :
-            float dt = t.read();
-            t.reset();
-            odometry.update(dt);
-            asserv.update(dt);
-            wait_ms(10);            
-        }
     #endif
 }
+
+void update()
+{
+    float dt = t.read();
+    t.reset();
+    
+    odometry.update(dt);
+    
+    #ifdef PLAN_A
+        instanceAsserv.update(dt);
+        float phi_r = (float)instanceAsserv.getPhiR();
+        float phi_l = (float)instanceAsserv.getPhiL();
+        float phi_max = (float)instanceAsserv.getPhiMax();
+        
+        motorR.setSpeed(0.08+(phi_r/phi_max));
+        motorL.setSpeed(0.08+(phi_l/phi_max));
+    #else
+        asserv.update(dt);
+    #endif
+}
+