Diff: main.cpp

Revision:

1:f3f702086a30

Parent:

0:7737d7573e3b

Child:

2:9d280856a536

--- a/main.cpp	Wed May 10 16:11:35 2017 +0000
+++ b/main.cpp	Thu May 11 11:49:50 2017 +0000
@@ -1,6 +1,10 @@
 #include "mbed.h"
+#include "AX12.h"
+#include "cmsis.h"
 
 #define SIZE_FIFO 20
+#define TIME 1
+#define T_MOT 0.00005
 
 /*
 DigitalIn IO1(p23);
@@ -25,23 +29,47 @@
 DigitalIn pressionGauche(p23);
 DigitalIn pressionDroit(p24);
 
-PwmOut moteurGauche(p21);
-PwmOut moteurDroit(p22);
-
 AnalogIn telemetre(p15);
 
 DigitalIn jack(p25);
 
-
+PwmOut motGauche(p21);
+PwmOut motDroit(p22);
 
 DigitalOut led(LED1);
 
 CAN can1(p30,p29); // Rx&Tx pour le CAN
 CANMessage msgRxBuffer[SIZE_FIFO]; // buffer en réception pour le CAN
 unsigned char FIFO_ecriture=0; //Position du fifo pour la reception CAN
+signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
 
-signed char FIFO_lecture=0;//Position du fifo de lecture des messages CAN
-int printCapteurs = 0;
+AX12 *AX12_17, *AX12_14, *multiple_AX12; 
+AX12 *AX12_14_2, *AX12_11, *multiple_AX12_2; 
+AX12 *AX12_4, *AX12_18, *multiple_AX12_3; 
+
+int printCapteurs = 1;
+
+static char TAB1[10]=   {0x0E, 0x00, 0x00, 0xFF, 0x03,          
+                         0x11, 0x50, 0x02, 0xFF, 0x03}; 
+                         
+static char TAB2[10]=   {0x0E, 0x50, 0x02, 0xFF, 0x03,                        
+                         0x11, 0x00, 0x00, 0xFF, 0x03};
+
+static char TAB3[10]=   {0x0E, 0x00, 0x00, 0xFF, 0x03,          
+                         0x0B, 0x50, 0x02, 0xFF, 0x03}; 
+                         
+static char TAB4[10]=   {0x0E, 0x50, 0x02, 0xFF, 0x03,                        
+                         0x0B, 0x00, 0x00, 0xFF, 0x03};
+
+static char TAB5[10]=   {0x04, 0x00, 0x00, 0xFF, 0x03,          
+                         0x12, 0x50, 0x02, 0xFF, 0x03}; 
+                         
+static char TAB6[10]=   {0x04, 0x50, 0x02, 0xFF, 0x03,                        
+                         0x12, 0x00, 0x00, 0xFF, 0x03};
+
+Timer t;
+Ticker flipper;
+
 
 void canRx_ISR (void)
 {
@@ -67,6 +95,49 @@
     wait_us(200);
 }
 
+/*********************************************************************************************************/
+/* FUNCTION NAME: AX1                                                                */
+/* DESCRIPTION  : bouge les AX12                                                                         */
+/*********************************************************************************************************/
+void AX1(void){
+    multiple_AX12->multiple_goal_and_speed(2,TAB1);
+    wait(TIME);
+    multiple_AX12->multiple_goal_and_speed(2,TAB2);
+    wait(TIME); 
+}
+
+/*********************************************************************************************************/
+/* FUNCTION NAME: AX2                                                                                    */
+/* DESCRIPTION  : bouge les AX12                                                                         */
+/*********************************************************************************************************/
+void AX2(void){
+    multiple_AX12_2->multiple_goal_and_speed(2,TAB3);
+    wait(TIME);
+    multiple_AX12_2->multiple_goal_and_speed(2,TAB4);
+    wait(TIME); 
+}
+
+/*********************************************************************************************************/
+/* FUNCTION NAME: AX3                                                                                    */
+/* DESCRIPTION  : bouge les AX12                                                                         */
+/*********************************************************************************************************/
+void AX3(void){
+    multiple_AX12_3->multiple_goal_and_speed(2,TAB5);
+    wait(TIME);
+    multiple_AX12_3->multiple_goal_and_speed(2,TAB6);
+    wait(TIME); 
+}
+
+/*********************************************************************************************************/
+/* FUNCTION NAME: moteur                                                                                 */
+/* DESCRIPTION  : bouge les moteurs                                                                      */
+/*********************************************************************************************************/
+void moteur(void){
+    motGauche.write(0.5f);
+    motDroit.write(0.5f); 
+}
+
+
 /****************************************************************************************/
 /* FUNCTION NAME: canProcessRx                                                          */
 /* DESCRIPTION  : Fonction de traitement des messages CAN                               */
@@ -103,19 +174,43 @@
     can1.attach(&canRx_ISR); // création de l'interrupt attachée à la réception sur le CAN
     
     CANMessage msgTx=CANMessage();
-
+    
+    
+    AX12_14 = new AX12(p9, p10, 14, 1000000); // tx, rx  
+    AX12_17 = new AX12(p9, p10, 17, 1000000);
+    multiple_AX12 = new AX12(p9,p10,0xFE,1000000); 
+    
+    
+    AX12_14_2 = new AX12(p13, p14, 14, 1000000);
+    AX12_11 = new AX12(p13, p14, 11, 1000000);
+    multiple_AX12_2 = new AX12(p13,p14,0xFE,1000000);
+    
+    AX12_4= new AX12(p28, p27, 14, 1000000);
+    AX12_18= new AX12(p28, p27, 11, 1000000);
+    multiple_AX12_3= new AX12(p28,p27,0xFE,1000000);
+    
+    motGauche.period(T_MOT);
+    motDroit.period(T_MOT);
+    motGauche.write(0.0);
+    motDroit.write(0.0);
+    
     while(1) {
         led = !led;
 
         canProcessRx();
+        
         if (printCapteurs){
+           
+            motGauche.write(0);
+            motDroit.write(0);
+            
             msgTx.id=0x21;
             msgTx.format=CANStandard;
             msgTx.type=CANData;
             msgTx.len=7;
             
             msgTx.data[0]=(unsigned char)cptGauche.read();
-            msgTx.data[1]=(unsigned char)cptDroit.read();
+            msgTx.data[1]=(unsigned char)cptDroit.read(); 
             msgTx.data[2]=(unsigned char)pressionGauche.read();
             msgTx.data[3]=(unsigned char)pressionDroit.read();
             msgTx.data[4]=(unsigned char)jack.read();
@@ -123,9 +218,17 @@
             msgTx.data[6]=(unsigned char)telemetre.read_u16();
                            
             can1.write(msgTx);
+            wait(0.2);
+        }else{
+            moteur();
             
+            AX1();   
+            AX2();
+            AX3();
+ 
         }
-        wait(0.5); 
+        
+         
     }
 }