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