Diff: main.cpp

Revision:

12:5e342c364d61

Parent:

11:90554d22ade5

--- a/main.cpp	Sat Nov 15 21:46:59 2014 +0000
+++ b/main.cpp	Thu Jul 21 13:12:39 2016 +0000
@@ -1,26 +1,89 @@
 #include "mbed.h"
+#include "SDC21XX_Motor.h"
+#include "rtos.h"
 #include "EthernetInterface.h"
- 
 #include "NetworkAPI/buffer.hpp"
 #include "NetworkAPI/select.hpp"
 #include "NetworkAPI/ip/address.hpp"
 #include "NetworkAPI/tcp/socket.hpp"
 using namespace network;
  
-#define MAX_CLIENTS 5
+#define MAX_CLIENTS 1
+#define SERVER_PORT 11556
+#define PERIODE_ENVOI 2.0
+
+#define REDUC_M1 1
+#define REDUC_M2 1
+#define REDUC_M3 1
+#define REDUC_M4 1
+#define REDUC_M5 1
+#define REDUC_M6 1
+
+Timer t, t2;
+Mutex mutex1;
+
+CAN CanBus(P0_4,P0_5);
+
+SDC21XX_Motor Motor1(1, &CanBus, 1, REDUC_M1, 1000);
+SDC21XX_Motor Motor2(1, &CanBus, 2, REDUC_M2, 1000);
+SDC21XX_Motor Motor3(2, &CanBus, 1, REDUC_M3, 1000);
+SDC21XX_Motor Motor4(2, &CanBus, 2, REDUC_M4, 1000);
+SDC21XX_Motor Motor5(3, &CanBus, 1, REDUC_M5, 1000);
+SDC21XX_Motor Motor6(3, &CanBus, 2, REDUC_M6, 1000);
+
+SDC21XX_Motor Moteur[6] = {Motor1, Motor2, Motor3, Motor4, Motor5, Motor6};
+
+uint8_t client_connected = 0, move_increment = 0, move_increment_val = 0; 
+
+tcp::Socket server;
+tcp::Socket client[MAX_CLIENTS];
+tcp::Socket *socket = NULL;
+
+
+void periodic_encoder_isr(void const *args) {
+    char reply[45], i = 0;
+    long dummy;
+
+    while(1)
+    {
+        mutex1.lock();
+        printf("Ticker\n\r");
+    
+        reply[0] = 0xa5;
+        for(i = 1;i<6;i++)
+            reply[7*i] = 0xa6;
+        for(i=0;i<6;i++)
+        {
+            reply[1+i*7] = 0x11;
+            reply[2+i*7] = i+1;
+            dummy = Moteur[i].GetRB()->ReadAbsoluteEncoderCount(Moteur[i].GetChannel());
+            memcpy(&reply[3+i*7], &dummy, 4);
+        }
+        reply[42] = 0xa7;
+        
+        client[0].write((void *)reply, 43);
+        Thread::wait(200);
+        mutex1.unlock();
+    }
+}
+
  
+int Process_Data_TCP_IP(uint8_t * data, tcp::Socket * client);
+    
+ 
+
 int
 main()
 {
+    mutex1.lock();
+    Thread thread1(periodic_encoder_isr);
+    
     EthernetInterface interface;
     interface.init();
     interface.connect();
     printf("IP Address is %s\n\r", interface.getIPAddress());
      
     Select select;
-    tcp::Socket server;
-    tcp::Socket client[MAX_CLIENTS];
-    tcp::Socket *socket = NULL;
      
     int result = 0;
     int index = 0;
@@ -30,7 +93,7 @@
      
     // Configure the server socket (assume everty thing works)
     server.open();
-    server.bind(1234);
+    server.bind(SERVER_PORT);
     server.listen(MAX_CLIENTS);
    
     // Add sockets to the select api
@@ -74,6 +137,9 @@
                      
                 // Send a nice message to the client
                 client[index].write((void *)message.data(), message.size());
+                client_connected = 1;
+                mutex1.unlock();
+                
                 continue;
             }
              
@@ -84,7 +150,8 @@
                     printf("Client disconnected %s:%d\n\r",
                         socket->getRemoteEndpoint().getAddress().toString().c_str(),
                         socket->getRemoteEndpoint().getPort());
-                     
+                    client_connected = 0;
+                    mutex1.lock();
                     // Close socket
                     socket->close();
                     break;
@@ -100,9 +167,108 @@
                         socket->getRemoteEndpoint().getPort());
                          
                     printf("%s\n\r", (char *)buffer.data());
+                    Process_Data_TCP_IP((uint8_t *)buffer.data(), client);
                     break;
             }
         }
-             
+        if(move_increment)
+        {
+             if(t.read_ms() > 100)
+             {
+                Moteur[move_increment].GetRB()->SetPosRelative(move_increment_val, Moteur[move_increment].GetChannel());
+                t.reset();
+                t.start();
+             }
+             else if(t2.read_ms() > 500)
+            {
+                move_increment = 0;
+                move_increment_val = 0;
+            }   
+        }
+
     } while (server.getStatus() == network::Socket::Listening);
-}
\ No newline at end of file
+}
+
+
+
+
+int Process_Data_TCP_IP(uint8_t * data, tcp::Socket * client)
+{
+    uint8_t nb_com = 0, cpt_com = 0, cpt_reply = 0;
+    int32_t param[3], dummy;
+    uint8_t reply[46];
+    
+    reply[0] = 0xa5;
+    cpt_reply ++;
+    
+    //Check Start byte
+    if(data[0] != 0xa5)
+        return -1;
+    
+    switch(data[1 + cpt_com])
+    {
+        case 0x01 :
+            param[0] = data[cpt_com + 3] | data[cpt_com + 4] << 8 | data[cpt_com + 5] << 16 | data[cpt_com + 6] << 24;
+            param[1] = data[cpt_com + 7] | data[cpt_com + 8] << 8 | data[cpt_com + 9] << 16 | data[cpt_com + 10] << 24;
+            param[2] = data[cpt_com + 11] | data[cpt_com + 12] << 8 | data[cpt_com + 13] << 16 | data[cpt_com + 14] << 24;
+            Moteur[data[cpt_com + 2] - 1].Setparameters(param[0], param[1], param[2]);
+            
+        break;
+        
+        case 0x02 :
+            param[0] = data[cpt_com + 3] | data[cpt_com + 4] << 8 | data[cpt_com + 5] << 16 | data[cpt_com + 6] << 24;
+            Moteur[data[cpt_com + 2] - 1].GetRB()->SetPosition(param[0], Moteur[data[cpt_com + 2]].GetChannel());
+                    
+        break;
+        
+        case 0x03 :
+            move_increment = data[cpt_com + 2] - 1;
+            t.reset();
+            t.start();
+            t2.reset();
+            t2.start();
+            move_increment_val = data[cpt_com + 3] | data[cpt_com + 4] << 8 | data[cpt_com + 5] << 16 | data[cpt_com + 6] << 24;
+            
+        break;
+        
+        case 0x11 :
+            if(cpt_reply > 1)
+            {
+                reply[cpt_reply] = 0xa6;
+                cpt_reply++;
+            }
+            reply[cpt_reply] = data[cpt_com + 2];
+            cpt_reply++;
+            dummy = Moteur[data[cpt_com + 2] - 1].GetRB()->ReadAbsoluteEncoderCount(Moteur[data[cpt_com + 2]].GetChannel());
+            memcpy(&reply[cpt_reply], &dummy, 4);
+            cpt_reply += 4;
+                        
+        break;
+        
+        case 0x80 :
+//            Moteur[0]
+        
+        break;
+    }//end switch
+    reply[cpt_reply] = 0xa7;
+    reply[cpt_reply + 1] = '\0';
+    client->write((void *)reply, cpt_reply + 1);
+    
+    return 1;
+    
+}//end function   
+
+/********************/
+// Fonction qui permet de modifier l'adresse MAC de la carte
+// /!\ /!\ /!\ Le routeur doit être paramétré de façon adequate, pour attribuer une adresse IP définie à cette adresse MAC /!\ /!\ /!\
+//(Dans un souci de simplicité, principalement, mais aussi pour différencier la carte des autres appareils se connectant au réseau)
+extern "C" void mbed_mac_address(char *mac)
+{
+    mac[0] = 0x12;
+    mac[1] = 0x34;
+    mac[2] = 0x56;
+    mac[3] = 0x78;
+    mac[4] = 0x9A;
+    mac[5] = 0xBC;
+};
+