Diff: Nucleo_L432KC_CAN3_BNO_Servo_EW485A.cpp

Revision:

0:ee3eb98ec375

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Nucleo_L432KC_CAN3_BNO_Servo_EW485A.cpp	Thu Jul 22 23:05:18 2021 +0000
@@ -0,0 +1,113 @@
+/* mbed Microcontroller Library
+ * Copyright (c) 2019 ARM Limited
+ * SPDX-License-Identifier: Apache-2.0
+ */
+// Program to test the CAN bus using SPI on the L432KC Nucleo board
+// to an MCP2551 CAN transceiver bus IC using https://os.mbed.com/users/tecnosys/code/mcp2515/
+
+// J. Bradshaw 20210512
+#include "mbed.h"
+#include "platform/mbed_thread.h"
+#include "CAN3.h"
+#include "BNO055.h"
+#include "ServoOut.h"
+
+#define THIS_CAN_ID     0x05   //Address of this CAN device
+#define DEST_CAN_ID     0   //Address of destination
+
+Serial pc(USBTX, USBRX);    //pc serial (tx, rx) uses USB PA_9 and PA_10 on Nucleo D1 and D0 pins
+BNO055 bno(D4, D5);
+SPI spi(D11, D12, D13);   // mosi, miso, sclk
+CAN3 can3(spi, D10, D2);         // spi bus, CS for MCP2515 controller
+ServoOut servoOut1(PA_0);   //A0);     // PA_0 is the servo output pulse
+AnalogIn ain3(A3);
+
+unsigned char can_txBufLen = 0;
+unsigned char can_tx_buf[8] = {0, 0, 0, 0, 0, 0, 0, 0};
+CANMessage canTx_msg;
+
+unsigned char can_rx_bufLen = 8;
+unsigned char can_rx_buf[8];
+unsigned short can_rxId = 0;
+CANMessage canRx_msg;
+
+Timer t;
+
+void bno_init(void){    
+    if(bno.check()){
+        pc.printf("BNO055 connected\r\n");
+        bno.setmode(OPERATION_MODE_CONFIG);
+        bno.SetExternalCrystal(1);
+        //bno.set_orientation(1);
+        bno.setmode(OPERATION_MODE_NDOF);  //Uses magnetometer
+        //bno.setmode(OPERATION_MODE_NDOF_FMC_OFF);   //no magnetometer
+        bno.set_angle_units(RADIANS);
+    }
+    else{
+        pc.printf("BNO055 NOT connected\r\n Program Trap.");
+        while(1);
+    }    
+}
+    
+int main() {        
+    thread_sleep_for(500);
+    
+    t.start();        
+    pc.baud(115200);
+    bno_init();
+    //can3.reset();            // reset the can bus interface
+    can3.frequency(500000);    // set up for 500K baudrate
+    
+    servoOut1.pulse_us = 1500;
+            
+    pc.printf("CAN MCP2515 test: %s\r\n", __FILE__);
+    while(1) {
+        
+        servoOut1.pulse_us = 1000 + (1000.0*ain3);
+        
+        bno.get_angles();
+        
+        pc.printf("%.2f %.2f %.2f\r\n",bno.euler.roll, bno.euler.pitch, bno.euler.yaw);
+        //sprintf(can_rx_buf, "b%.2f\r\n", bno.euler.yaw);    //format output message string
+        can_tx_buf[0] = bno.euler.rawroll & 0x00ff;
+        can_tx_buf[1] = (bno.euler.rawroll >> 8) & 0x00ff;
+        can_tx_buf[2] = bno.euler.rawpitch & 0x00ff;
+        can_tx_buf[3] = (bno.euler.rawpitch >> 8) & 0x00ff;
+        can_tx_buf[4] = bno.euler.rawyaw & 0x00ff;
+        can_tx_buf[5] = (bno.euler.rawyaw >> 8) & 0x00ff;
+        can_tx_buf[6] = 0;
+        can_tx_buf[7] = 0;
+
+        // CAN write message
+        for(int i=0;i<8;i++){
+            canTx_msg.data[i] = can_tx_buf[i];
+        }        
+        canTx_msg.id = THIS_CAN_ID; //(rand() % 0xff);    // Randomize transmit ID or  THIS_CAN_ID;
+        
+        can3.write(&canTx_msg);
+        pc.printf("%.2f CAN TX id=%02X data: ", t.read(), canTx_msg.id);
+        for(int i=0;i<8;i++){
+            pc.printf(" %2X", can_tx_buf[i]);
+        }
+        pc.printf("\r\n");
+        //set up a random delay time of up to .79 seconds
+        //delayT = ((rand() % 7) * 31.0)  + ((rand() % 9) * .1) + runTime.sec_total;                        
+        
+        // CAN receive message       
+        if(can3.read(&canRx_msg) == CAN_OK){ //if message is available, read into msg
+            pc.printf("CAN RX id=0x%02X data: ", canRx_msg.id);
+            for (int i = 0; i < canRx_msg.len; i++) {
+                pc.printf(" %2X", canRx_msg.data[i]);
+            }
+            pc.printf("\r\n");
+        }
+        
+        //if(canTxErrors > 50){            
+//            can.reset();
+//        }
+//        if(canRxErrors > 50){            
+//            can.reset();
+//        }
+        thread_sleep_for(50);     
+    }//while(1)
+}//main