USNA-UMBC Project Data (Yaw) Generator / Transmitter
Dependencies: ServoOut mcp2515 BNO055
NODE-KF-1.cpp
- Committer:
- professorrodriguezse
- Date:
- 2022-05-20
- Revision:
- 1:269a9d669b48
File content as of revision 1:269a9d669b48:
/* 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