A test for vescs

Revision:
1:9ecda2b969c2
Parent:
0:0da0fd430e20
Child:
2:12431dd6475f
--- a/main.cpp	Thu Dec 06 14:43:47 2018 +0000
+++ b/main.cpp	Fri Jun 14 15:08:55 2019 +0000
@@ -2,8 +2,10 @@
 
 const PinName can1rxPins[] = {PA_11};
 const PinName can1txPins[] = {PA_12};
-const PinName can2rxPins[] = {PB_12};
-const PinName can2txPins[] = {PB_13};
+const PinName can2rxPins[] = {PB_5};
+const PinName can2txPins[] = {PB_6};
+
+uint64_t counter=0;
 
 //CAN1
 //0,0=OK 0,1=OK 0,2=OK 1,0=OK 1,1=OK 1,2=OK 2,0=OK 2,1=OK 2,2=OK
@@ -22,27 +24,54 @@
 CANMessage messageIn;
 CANMessage messageOut;
 
+uint64_t cantoi(CANMessage message) {
+    int data=0;
+    for(int i=message.len-1, k=0; i>=0; i--, k++) {
+        data += message.data[i] << (8*k); 
+    }
+    return data;
+    //pose = message.data[0] + (message.data[1] << 8) + (message.data[2] << 16) + (message.data[3] << 24);
+}
+
+void itocan(uint64_t data, CANMessage& message) {
+    
+    for(int i=message.len-1; i>=0; i--) {
+        //printf("dataconv:\t% 12llu\n\r", data);
+        message.data[i] = data & 0x00000000000000ff;
+        data = data>>8; 
+    }
+}
+
 void canRxIsr()
 {
-    while(1)
-    {
-    if(can1.read(messageIn))
-     {
-      led1 = !led1;
-       printf("received\n\r");
-     }
+    while(1) {
+        if(can.read(messageIn)) {
+            led1 = !led1;
+            printf("received from %lu\t%llu\n\r", messageIn.id, cantoi(messageIn));
+        }
     }
-   }
+}
+
+//pose = messageIn.data[0] + (messageIn.data[1] << 8) + (messageIn.data[2] << 16) + (messageIn.data[3] << 24);
+
 void sendMessage()
 {
     int status = can.write(messageOut);
-   printf("Send status: %d\r\n", status);
+    printf("Send status: %d\r\n", status);
 }
 
 void sendMessageLoop()
 {
-    while (true)
-    {
+    while (true) {
+        counter += 1;
+        
+        messageOut.id = 1000;
+        messageOut.format = CANStandard;
+        messageOut.data[0]=25;
+        messageOut.len = 8;
+    
+        itocan(counter, messageOut);
+        printf("sending couter: %llu\t| CAN data: %llu\n\r", counter, cantoi(messageOut));
         sendMessage();
         osDelay(1000);
     }
@@ -52,11 +81,12 @@
 {
     
     printf("CAN receive / transmit test.\r\n");
-    //can.frequency(125000);
+    can.frequency(500000);
 
-    messageOut.id = 1337;
-    //messageOut.format = CANExtended;
-    //messageOut.len = 0;
+    messageOut.id = 1000;
+    messageOut.format = CANStandard;
+    messageOut.data[0]=24;
+    messageOut.len = 8;
 
     canrx.start(canRxIsr);
       printf("CAN receive / .\r\n");