USNA-UMBC Project Data (Yaw) Generator / Transmitter
Dependencies: ServoOut mcp2515 BNO055
Diff: NODE-KF-1.cpp
- Revision:
- 1:269a9d669b48
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/NODE-KF-1.cpp Fri May 20 18:38:44 2022 +0000 @@ -0,0 +1,100 @@ +/* +Sends and Reads position of servos in degrees and prints them all. + */ + +#include "mbed.h" +#include "platform/mbed_thread.h" +#include "BNO055.h" +#include "CAN3.h" +#include "ServoOut.h" + +int myID = 1; + +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(A0); //A0); // PA_0 is the servo output pulse + +CANMessage canTx_msg; +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(DEGREES); + } else { + pc.printf("BNO055 NOT connected\r\n Program Trap."); + while(1); + } +} + +float unwrap(float previous_angle, float new_angle) +{ + float d = new_angle - previous_angle; + //d = d > 180 ? d - 2 * 180 : (d < -180 ? d + 2 * 180 : d); + if (d > 180) { + d = d - 360; + } else { + if (d < -180) { + d = d + 360; + } + } + return previous_angle + d; +} + +int main() +{ + servoOut1.pulseMin = 500; + servoOut1.pulseMax = 2500; + thread_sleep_for(50); + float yaw, oldYaw; + float currTime, dt; + int controlSignal; + float T = 17; //Period in ms + t.start(); + pc.baud(115200); + pc.printf("Starting Program... \n\r"); + bno_init(); + //can3.reset(); // reset the can bus interface + can3.frequency(500000); // set up for 500K baudrate + char msg_send[8]; + char msg_read_char[8]; + //servoOut1.pulse_us = 1500; + bno.get_angles(); + yaw = bno.euler.yaw; + pc.printf("Original Yaw: %.2f \r\n", yaw); + thread_sleep_for(1000); + + //pc.printf("CAN MCP2515 test: %s\r\n", __FILE__); + while(1) { + currTime = t.read(); + controlSignal = 1500 + (int)1000*(sin(currTime*2*3.14/10))*sin(currTime*2*3.14/26)*sin(currTime*2*3.14/26); + servoOut1 = controlSignal; + oldYaw = yaw; + bno.get_angles(); + yaw = unwrap(oldYaw,bno.euler.yaw); + //pc.printf("MyID: %d Raw Yaw Value: %.2f Unwrapped Yaw %.2f \r\n", myID, bno.euler.yaw, yaw); + + sprintf(msg_send, "%.1f\r\n", yaw); + for(int i=0; i<8; i++) { + canTx_msg.data[i] = msg_send[i]; + } + canTx_msg.id = myID; + can3.write(&canTx_msg); + pc.printf("Time: %.3f \t Angle: %.1f \t Control: %d\r\n", t.read(), yaw, controlSignal); + dt = T-(t.read()-currTime)*1000; + if (dt > 0) { + thread_sleep_for(dt); + } + + }//while(1) +}//main