Diff: main.cpp

Revision:

0:22b26272c47e

Child:

1:3e0ad6a257c5

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Mar 22 11:32:10 2020 +0000
@@ -0,0 +1,146 @@
+// Nucleo_CAN_master for AIRo-4.1
+// Created by Atsushi Kakogawa, 2020.03.20
+// Modified by ****** ******, 2020.**.**
+// Department of Robotics, Ritsumeikan University, Japan
+
+#include "mbed.h"
+#include "CAN.h"
+    
+DigitalOut led1(LED1);
+DigitalOut led2(D8);
+DigitalOut led3(D4);
+Serial pc(USBTX, USBRX, 115200);    // Serial PC
+
+Ticker rx; // timer for data receiver
+
+//cancom;
+CAN can(PA_11, PA_12);
+
+// from a master computer
+int i=0;
+char buf[255];
+
+// converter
+int mode, dir;
+char tx_data[8];
+
+// Data-receiver
+int id = 0;
+int flag = 0;
+
+// CAN communication to slaves
+void send(int id, int mode, int dir, char targ3, char targ2, char targ1) {
+    int target_ang = ((int)targ3-0x30)*100+((int)targ2-0x30)*10+(int)targ1-0x30;    // Convert from String to Int
+    char tx_target_U = (target_ang >> 8) & 0xff;                                    // Split the Int data to upper
+    char tx_target_L = target_ang & 0xff;                                           // Split the Int data to lower
+    tx_data[0] = id;             // ID
+    tx_data[1] = mode;          // mode (0: control, 1: response) 
+    tx_data[2] = dir;           // dir (0: positive, 1: negative)
+    tx_data[3] = tx_target_U;   // target value upper 8bit 
+    tx_data[4] = tx_target_L;   // target value lower 8bit
+    tx_data[5] = 0;
+    tx_data[6] = 0;
+    tx_data[7] = 0;
+    can.write(CANMessage(1337, tx_data, 5));
+    //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]);
+}
+
+void receive() {
+    CANMessage msg;
+    // CAN read from slaves and send to a master computer
+    if(can.read(msg)) {
+        int rid = msg.data[0];
+        int data1 = (msg.data[2] << 8) + msg.data[3];   // current
+        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\r\n", rid, data1, data2, data3);
+    }
+}
+
+// Data-receiver timer
+void RXdata() {
+    if (flag == 1) {
+        for (id=1; id<3; id++) {
+            send(id,1,0,0,0,0);                            // Receive command
+            receive();
+        }
+    }
+}
+
+int main() {
+    
+    //pc.printf("0,0\r\n");
+    can.frequency(1000000);
+    rx.attach(&RXdata, 0.2);  // 200 ms
+    
+    while(1) {
+
+        // Serial read from a master computer
+        if (pc.getc()) {
+            for (i=1; i<256; i++) {
+                buf[i] = pc.getc();
+                if (buf[i] == '\n') {
+                    
+                    // ***** Angle control mode *****
+                    // send(ID, mode(0), dir(0 or 1), 3rd digit, 2rd digit, 1rd digit)
+                    if (buf[i-5] == 'A'){ 
+                        send(buf[i-4]-0x30,0,0,buf[i-3],buf[i-2],buf[i-1]);
+                        
+                    // ***** Receive mode (single data) *****
+                    // send(ID, mode(1), dir(0), NC, NC, NC)
+                    } else if (buf[i-5] == 'B') {
+                        send(buf[i-4]-0x30,1,0,0,0,0);
+                        receive();
+                        
+                    // ***** Speed control mode (positive) *****
+                    // send(ID, mode(0), dir(0), 3rd digit, 2rd digit, 1rd digit)
+                    } else if (buf[i-5] == 'C') {
+                        send(buf[i-4]-0x30,0,0,buf[i-3],buf[i-2],buf[i-1]);
+                        
+                    // ***** Speed control mode (negative) *****
+                    // send(ID, mode(0), dir(1), 3rd digit, 2rd digit, 1rd digit)
+                    } else if (buf[i-5] == 'D') {
+                        send(buf[i-4]-0x30,0,1,buf[i-3],buf[i-2],buf[i-1]);
+                    
+                    // ***** 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;
+                        flag = 1;
+                    
+                    // ***** Receive mode stop (continuous data) *****
+                    // send(ID, mode(1), dir(0), NC, NC, NC)
+                    } else if (buf[i-5] == 'F') {
+                        id = 0;
+                        flag = 0;
+                    }
+                    i = 1;
+                    break;
+                }
+            }
+            led1=!led1;
+            led2=!led2;
+             
+            //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);
+        } else {
+            led3=0;
+        }
+        if(can.tderror()){
+            pc.printf("tx error,%d\r\n",can.tderror());
+            can.reset();
+            led3=1;
+            wait (0.1);
+        } else {
+            led3=0;
+        }
+    }
+}
\ No newline at end of file