Erick Rodriguez / Mbed OS USNA-UMBC-KF-01

USNA-UMBC Project Data (Yaw) Generator / Transmitter

Dependencies:   ServoOut mcp2515 BNO055

Nucleo_L432KC_CAN3_BNO_Servo_EW485A.cpp@0:ee3eb98ec375, 2021-07-22 (annotated)

Committer:
jebradshaw
Date:
Thu Jul 22 23:05:18 2021 +0000
Revision:
0:ee3eb98ec375
Nucleo EW485B test for BNO-055, CAN mcp2515 hardware, servo, etc

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jebradshaw 0:ee3eb98ec375 1 /* mbed Microcontroller Library
jebradshaw 0:ee3eb98ec375 2 * Copyright (c) 2019 ARM Limited
jebradshaw 0:ee3eb98ec375 3 * SPDX-License-Identifier: Apache-2.0
jebradshaw 0:ee3eb98ec375 4 */
jebradshaw 0:ee3eb98ec375 5 // Program to test the CAN bus using SPI on the L432KC Nucleo board
jebradshaw 0:ee3eb98ec375 6 // to an MCP2551 CAN transceiver bus IC using https://os.mbed.com/users/tecnosys/code/mcp2515/
jebradshaw 0:ee3eb98ec375 7
jebradshaw 0:ee3eb98ec375 8 // J. Bradshaw 20210512
jebradshaw 0:ee3eb98ec375 9 #include "mbed.h"
jebradshaw 0:ee3eb98ec375 10 #include "platform/mbed_thread.h"
jebradshaw 0:ee3eb98ec375 11 #include "CAN3.h"
jebradshaw 0:ee3eb98ec375 12 #include "BNO055.h"
jebradshaw 0:ee3eb98ec375 13 #include "ServoOut.h"
jebradshaw 0:ee3eb98ec375 14
jebradshaw 0:ee3eb98ec375 15 #define THIS_CAN_ID 0x05 //Address of this CAN device
jebradshaw 0:ee3eb98ec375 16 #define DEST_CAN_ID 0 //Address of destination
jebradshaw 0:ee3eb98ec375 17
jebradshaw 0:ee3eb98ec375 18 Serial pc(USBTX, USBRX); //pc serial (tx, rx) uses USB PA_9 and PA_10 on Nucleo D1 and D0 pins
jebradshaw 0:ee3eb98ec375 19 BNO055 bno(D4, D5);
jebradshaw 0:ee3eb98ec375 20 SPI spi(D11, D12, D13); // mosi, miso, sclk
jebradshaw 0:ee3eb98ec375 21 CAN3 can3(spi, D10, D2); // spi bus, CS for MCP2515 controller
jebradshaw 0:ee3eb98ec375 22 ServoOut servoOut1(PA_0); //A0); // PA_0 is the servo output pulse
jebradshaw 0:ee3eb98ec375 23 AnalogIn ain3(A3);
jebradshaw 0:ee3eb98ec375 24
jebradshaw 0:ee3eb98ec375 25 unsigned char can_txBufLen = 0;
jebradshaw 0:ee3eb98ec375 26 unsigned char can_tx_buf[8] = {0, 0, 0, 0, 0, 0, 0, 0};
jebradshaw 0:ee3eb98ec375 27 CANMessage canTx_msg;
jebradshaw 0:ee3eb98ec375 28
jebradshaw 0:ee3eb98ec375 29 unsigned char can_rx_bufLen = 8;
jebradshaw 0:ee3eb98ec375 30 unsigned char can_rx_buf[8];
jebradshaw 0:ee3eb98ec375 31 unsigned short can_rxId = 0;
jebradshaw 0:ee3eb98ec375 32 CANMessage canRx_msg;
jebradshaw 0:ee3eb98ec375 33
jebradshaw 0:ee3eb98ec375 34 Timer t;
jebradshaw 0:ee3eb98ec375 35
jebradshaw 0:ee3eb98ec375 36 void bno_init(void){
jebradshaw 0:ee3eb98ec375 37 if(bno.check()){
jebradshaw 0:ee3eb98ec375 38 pc.printf("BNO055 connected\r\n");
jebradshaw 0:ee3eb98ec375 39 bno.setmode(OPERATION_MODE_CONFIG);
jebradshaw 0:ee3eb98ec375 40 bno.SetExternalCrystal(1);
jebradshaw 0:ee3eb98ec375 41 //bno.set_orientation(1);
jebradshaw 0:ee3eb98ec375 42 bno.setmode(OPERATION_MODE_NDOF); //Uses magnetometer
jebradshaw 0:ee3eb98ec375 43 //bno.setmode(OPERATION_MODE_NDOF_FMC_OFF); //no magnetometer
jebradshaw 0:ee3eb98ec375 44 bno.set_angle_units(RADIANS);
jebradshaw 0:ee3eb98ec375 45 }
jebradshaw 0:ee3eb98ec375 46 else{
jebradshaw 0:ee3eb98ec375 47 pc.printf("BNO055 NOT connected\r\n Program Trap.");
jebradshaw 0:ee3eb98ec375 48 while(1);
jebradshaw 0:ee3eb98ec375 49 }
jebradshaw 0:ee3eb98ec375 50 }
jebradshaw 0:ee3eb98ec375 51
jebradshaw 0:ee3eb98ec375 52 int main() {
jebradshaw 0:ee3eb98ec375 53 thread_sleep_for(500);
jebradshaw 0:ee3eb98ec375 54
jebradshaw 0:ee3eb98ec375 55 t.start();
jebradshaw 0:ee3eb98ec375 56 pc.baud(115200);
jebradshaw 0:ee3eb98ec375 57 bno_init();
jebradshaw 0:ee3eb98ec375 58 //can3.reset(); // reset the can bus interface
jebradshaw 0:ee3eb98ec375 59 can3.frequency(500000); // set up for 500K baudrate
jebradshaw 0:ee3eb98ec375 60
jebradshaw 0:ee3eb98ec375 61 servoOut1.pulse_us = 1500;
jebradshaw 0:ee3eb98ec375 62
jebradshaw 0:ee3eb98ec375 63 pc.printf("CAN MCP2515 test: %s\r\n", __FILE__);
jebradshaw 0:ee3eb98ec375 64 while(1) {
jebradshaw 0:ee3eb98ec375 65
jebradshaw 0:ee3eb98ec375 66 servoOut1.pulse_us = 1000 + (1000.0*ain3);
jebradshaw 0:ee3eb98ec375 67
jebradshaw 0:ee3eb98ec375 68 bno.get_angles();
jebradshaw 0:ee3eb98ec375 69
jebradshaw 0:ee3eb98ec375 70 pc.printf("%.2f %.2f %.2f\r\n",bno.euler.roll, bno.euler.pitch, bno.euler.yaw);
jebradshaw 0:ee3eb98ec375 71 //sprintf(can_rx_buf, "b%.2f\r\n", bno.euler.yaw); //format output message string
jebradshaw 0:ee3eb98ec375 72 can_tx_buf[0] = bno.euler.rawroll & 0x00ff;
jebradshaw 0:ee3eb98ec375 73 can_tx_buf[1] = (bno.euler.rawroll >> 8) & 0x00ff;
jebradshaw 0:ee3eb98ec375 74 can_tx_buf[2] = bno.euler.rawpitch & 0x00ff;
jebradshaw 0:ee3eb98ec375 75 can_tx_buf[3] = (bno.euler.rawpitch >> 8) & 0x00ff;
jebradshaw 0:ee3eb98ec375 76 can_tx_buf[4] = bno.euler.rawyaw & 0x00ff;
jebradshaw 0:ee3eb98ec375 77 can_tx_buf[5] = (bno.euler.rawyaw >> 8) & 0x00ff;
jebradshaw 0:ee3eb98ec375 78 can_tx_buf[6] = 0;
jebradshaw 0:ee3eb98ec375 79 can_tx_buf[7] = 0;
jebradshaw 0:ee3eb98ec375 80
jebradshaw 0:ee3eb98ec375 81 // CAN write message
jebradshaw 0:ee3eb98ec375 82 for(int i=0;i<8;i++){
jebradshaw 0:ee3eb98ec375 83 canTx_msg.data[i] = can_tx_buf[i];
jebradshaw 0:ee3eb98ec375 84 }
jebradshaw 0:ee3eb98ec375 85 canTx_msg.id = THIS_CAN_ID; //(rand() % 0xff); // Randomize transmit ID or THIS_CAN_ID;
jebradshaw 0:ee3eb98ec375 86
jebradshaw 0:ee3eb98ec375 87 can3.write(&canTx_msg);
jebradshaw 0:ee3eb98ec375 88 pc.printf("%.2f CAN TX id=%02X data: ", t.read(), canTx_msg.id);
jebradshaw 0:ee3eb98ec375 89 for(int i=0;i<8;i++){
jebradshaw 0:ee3eb98ec375 90 pc.printf(" %2X", can_tx_buf[i]);
jebradshaw 0:ee3eb98ec375 91 }
jebradshaw 0:ee3eb98ec375 92 pc.printf("\r\n");
jebradshaw 0:ee3eb98ec375 93 //set up a random delay time of up to .79 seconds
jebradshaw 0:ee3eb98ec375 94 //delayT = ((rand() % 7) * 31.0) + ((rand() % 9) * .1) + runTime.sec_total;
jebradshaw 0:ee3eb98ec375 95
jebradshaw 0:ee3eb98ec375 96 // CAN receive message
jebradshaw 0:ee3eb98ec375 97 if(can3.read(&canRx_msg) == CAN_OK){ //if message is available, read into msg
jebradshaw 0:ee3eb98ec375 98 pc.printf("CAN RX id=0x%02X data: ", canRx_msg.id);
jebradshaw 0:ee3eb98ec375 99 for (int i = 0; i < canRx_msg.len; i++) {
jebradshaw 0:ee3eb98ec375 100 pc.printf(" %2X", canRx_msg.data[i]);
jebradshaw 0:ee3eb98ec375 101 }
jebradshaw 0:ee3eb98ec375 102 pc.printf("\r\n");
jebradshaw 0:ee3eb98ec375 103 }
jebradshaw 0:ee3eb98ec375 104
jebradshaw 0:ee3eb98ec375 105 //if(canTxErrors > 50){
jebradshaw 0:ee3eb98ec375 106 // can.reset();
jebradshaw 0:ee3eb98ec375 107 // }
jebradshaw 0:ee3eb98ec375 108 // if(canRxErrors > 50){
jebradshaw 0:ee3eb98ec375 109 // can.reset();
jebradshaw 0:ee3eb98ec375 110 // }
jebradshaw 0:ee3eb98ec375 111 thread_sleep_for(50);
jebradshaw 0:ee3eb98ec375 112 }//while(1)
jebradshaw 0:ee3eb98ec375 113 }//main