Diff: main.cpp

Revision:

1:3e0ad6a257c5

Parent:

0:22b26272c47e

--- a/main.cpp	Sun Mar 22 11:32:10 2020 +0000
+++ b/main.cpp	Fri Mar 05 08:38:30 2021 +0000
@@ -1,4 +1,19 @@
+
 // Nucleo_CAN_master for AIRo-4.1
+
+
+
+
+
+
+
+
+
+
+
+
+
+
 // Created by Atsushi Kakogawa, 2020.03.20
 // Modified by ****** ******, 2020.**.**
 // Department of Robotics, Ritsumeikan University, Japan
@@ -16,6 +31,9 @@
 //cancom;
 CAN can(PA_11, PA_12);
 
+// motor number
+int m_num = 16;
+
 // from a master computer
 int i=0;
 char buf[255];
@@ -25,7 +43,7 @@
 char tx_data[8];
 
 // Data-receiver
-int id = 0;
+int id = 0, j;
 int flag = 0;
 
 // CAN communication to slaves
@@ -41,7 +59,7 @@
     tx_data[5] = 0;
     tx_data[6] = 0;
     tx_data[7] = 0;
-    can.write(CANMessage(1337, tx_data, 5));
+    can.write(CANMessage(1335, tx_data, 8));
     //pc.printf("Sent(0x): %x %x %x %x %x %x %x %x\r\n", tx_data[0], tx_data[1], tx_data[2], tx_data[3], tx_data[4], tx_data[5], tx_data[6], tx_data[7]);
 }
 
@@ -54,27 +72,36 @@
         int data2 = (msg.data[4] << 8) + msg.data[5];   // velocity
         int data3 = (msg.data[6] << 8) + msg.data[7];   // angle
         //pc.printf("%d,%.2f,%.2f,%.2f\r\n", id, data1, data2, data3);
+        //pc.printf("%d,%d,%d,%d,%d\r\n", rid, data1, data2, data3, 999);
         pc.printf("%d,%d,%d,%d\r\n", rid, data1, data2, data3);
+        //wait (0.001);//追加分
     }
 }
 
 // Data-receiver timer
 void RXdata() {
     if (flag == 1) {
-        for (id=1; id<3; id++) {
-            send(id,1,0,0,0,0);                            // Receive command
-            receive();
+        send(id,1,0,0,0,0);                            // Receive command
+        //wait (0.001); 
+        receive();
+        //wait (0.001); 
+        if (id == m_num) {
+            id = 1;
+        } else {
+            id++;
         }
     }
+    //wait (0.001);//追加分
 }
 
 int main() {
-    
+    //pc.attach(NULL);//割り込み解除（試験的に導入，2021.2.7, oka）
     //pc.printf("0,0\r\n");
     can.frequency(1000000);
-    rx.attach(&RXdata, 0.2);  // 200 ms
+    rx.attach(&RXdata, 0.02);  // 50 ms
     
     while(1) {
+        //receive();
 
         // Serial read from a master computer
         if (pc.getc()) {
@@ -106,13 +133,16 @@
                     // ***** Receive mode start (continuous data) *****
                     // send(ID, mode(1), dir(0), NC, NC, NC)
                     } else if (buf[i-5] == 'E') {
-                        id = buf[i-4]-0x30;
+                        //id = buf[i-4]-0x30;
+                        //m_num = ((int)buf[i-3]-0x30)*100+((int)buf[i-2]-0x30)*10+(int)buf[i-1]-0x30;    // Convert from String to Int;
+                        id = ((int)buf[i-3]-0x30)*100+((int)buf[i-2]-0x30)*10+(int)buf[i-1]-0x30;    // Convert from String to Int;
                         flag = 1;
                     
                     // ***** Receive mode stop (continuous data) *****
                     // send(ID, mode(1), dir(0), NC, NC, NC)
                     } else if (buf[i-5] == 'F') {
                         id = 0;
+                        //m_num = 0;
                         flag = 0;
                     }
                     i = 1;
@@ -121,16 +151,17 @@
             }
             led1=!led1;
             led2=!led2;
+            
+            //pc.attach(NULL);//割り込み解除（試験的に導入，2021.2.7, oka）
              
             //wait (0.001); // Eriminate this "wait" when linux
         }
-        
         // CAN error check
         if(can.rderror()){
             pc.printf("rx error,%d\r\n",can.rderror());
             can.reset();
             led3=1;
-            wait (0.1);
+            //wait (0.1);
         } else {
             led3=0;
         }
@@ -138,7 +169,7 @@
             pc.printf("tx error,%d\r\n",can.tderror());
             can.reset();
             led3=1;
-            wait (0.1);
+            //wait (0.1);
         } else {
             led3=0;
         }