![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
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?
User | Revision | Line number | New 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 |