Revision 1:269a9d669b48, committed 2022-05-20

Comitter:

professorrodriguezse

Date:

Fri May 20 18:38:44 2022 +0000

Parent:

0:ee3eb98ec375

Commit message:

Data Generator

Changed in this revision

--- /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

--- a/Nucleo_L432KC_CAN3_BNO_Servo_EW485A.cpp	Thu Jul 22 23:05:18 2021 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,113 +0,0 @@
-/* mbed Microcontroller Library
- * Copyright (c) 2019 ARM Limited
- * SPDX-License-Identifier: Apache-2.0
- */
-// Program to test the CAN bus using SPI on the L432KC Nucleo board
-// to an MCP2551 CAN transceiver bus IC using https://os.mbed.com/users/tecnosys/code/mcp2515/
-
-// J. Bradshaw 20210512
-#include "mbed.h"
-#include "platform/mbed_thread.h"
-#include "CAN3.h"
-#include "BNO055.h"
-#include "ServoOut.h"
-
-#define THIS_CAN_ID     0x05   //Address of this CAN device
-#define DEST_CAN_ID     0   //Address of destination
-
-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(PA_0);   //A0);     // PA_0 is the servo output pulse
-AnalogIn ain3(A3);
-
-unsigned char can_txBufLen = 0;
-unsigned char can_tx_buf[8] = {0, 0, 0, 0, 0, 0, 0, 0};
-CANMessage canTx_msg;
-
-unsigned char can_rx_bufLen = 8;
-unsigned char can_rx_buf[8];
-unsigned short can_rxId = 0;
-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(RADIANS);
-    }
-    else{
-        pc.printf("BNO055 NOT connected\r\n Program Trap.");
-        while(1);
-    }    
-}
-    
-int main() {        
-    thread_sleep_for(500);
-    
-    t.start();        
-    pc.baud(115200);
-    bno_init();
-    //can3.reset();            // reset the can bus interface
-    can3.frequency(500000);    // set up for 500K baudrate
-    
-    servoOut1.pulse_us = 1500;
-            
-    pc.printf("CAN MCP2515 test: %s\r\n", __FILE__);
-    while(1) {
-        
-        servoOut1.pulse_us = 1000 + (1000.0*ain3);
-        
-        bno.get_angles();
-        
-        pc.printf("%.2f %.2f %.2f\r\n",bno.euler.roll, bno.euler.pitch, bno.euler.yaw);
-        //sprintf(can_rx_buf, "b%.2f\r\n", bno.euler.yaw);    //format output message string
-        can_tx_buf[0] = bno.euler.rawroll & 0x00ff;
-        can_tx_buf[1] = (bno.euler.rawroll >> 8) & 0x00ff;
-        can_tx_buf[2] = bno.euler.rawpitch & 0x00ff;
-        can_tx_buf[3] = (bno.euler.rawpitch >> 8) & 0x00ff;
-        can_tx_buf[4] = bno.euler.rawyaw & 0x00ff;
-        can_tx_buf[5] = (bno.euler.rawyaw >> 8) & 0x00ff;
-        can_tx_buf[6] = 0;
-        can_tx_buf[7] = 0;
-
-        // CAN write message
-        for(int i=0;i<8;i++){
-            canTx_msg.data[i] = can_tx_buf[i];
-        }        
-        canTx_msg.id = THIS_CAN_ID; //(rand() % 0xff);    // Randomize transmit ID or  THIS_CAN_ID;
-        
-        can3.write(&canTx_msg);
-        pc.printf("%.2f CAN TX id=%02X data: ", t.read(), canTx_msg.id);
-        for(int i=0;i<8;i++){
-            pc.printf(" %2X", can_tx_buf[i]);
-        }
-        pc.printf("\r\n");
-        //set up a random delay time of up to .79 seconds
-        //delayT = ((rand() % 7) * 31.0)  + ((rand() % 9) * .1) + runTime.sec_total;                        
-        
-        // CAN receive message       
-        if(can3.read(&canRx_msg) == CAN_OK){ //if message is available, read into msg
-            pc.printf("CAN RX id=0x%02X data: ", canRx_msg.id);
-            for (int i = 0; i < canRx_msg.len; i++) {
-                pc.printf(" %2X", canRx_msg.data[i]);
-            }
-            pc.printf("\r\n");
-        }
-        
-        //if(canTxErrors > 50){            
-//            can.reset();
-//        }
-//        if(canRxErrors > 50){            
-//            can.reset();
-//        }
-        thread_sleep_for(50);     
-    }//while(1)
-}//main