Erick Rodriguez / Mbed OS USNA-UMBC-KF-01

USNA-UMBC Project Data (Yaw) Generator / Transmitter

Dependencies:   ServoOut mcp2515 BNO055

NODE-KF-1.cpp@1:269a9d669b48, 23 months ago (annotated)

Committer:
professorrodriguezse
Date:
Fri May 20 18:38:44 2022 +0000
Revision:
1:269a9d669b48
Data Generator

Who changed what in which revision?

UserRevisionLine numberNew contents of line
professorrodriguezse 1:269a9d669b48 1 /*
professorrodriguezse 1:269a9d669b48 2 Sends and Reads position of servos in degrees and prints them all.
professorrodriguezse 1:269a9d669b48 3 */
professorrodriguezse 1:269a9d669b48 4
professorrodriguezse 1:269a9d669b48 5 #include "mbed.h"
professorrodriguezse 1:269a9d669b48 6 #include "platform/mbed_thread.h"
professorrodriguezse 1:269a9d669b48 7 #include "BNO055.h"
professorrodriguezse 1:269a9d669b48 8 #include "CAN3.h"
professorrodriguezse 1:269a9d669b48 9 #include "ServoOut.h"
professorrodriguezse 1:269a9d669b48 10
professorrodriguezse 1:269a9d669b48 11 int myID = 1;
professorrodriguezse 1:269a9d669b48 12
professorrodriguezse 1:269a9d669b48 13 Serial pc(USBTX, USBRX); //pc serial (tx, rx) uses USB PA_9 and PA_10 on Nucleo D1 and D0 pins
professorrodriguezse 1:269a9d669b48 14 BNO055 bno(D4, D5);
professorrodriguezse 1:269a9d669b48 15 SPI spi(D11, D12, D13); // mosi, miso, sclk
professorrodriguezse 1:269a9d669b48 16 CAN3 can3(spi, D10, D2); // spi bus, CS for MCP2515 controller
professorrodriguezse 1:269a9d669b48 17 ServoOut servoOut1(A0); //A0); // PA_0 is the servo output pulse
professorrodriguezse 1:269a9d669b48 18
professorrodriguezse 1:269a9d669b48 19 CANMessage canTx_msg;
professorrodriguezse 1:269a9d669b48 20 CANMessage canRx_msg;
professorrodriguezse 1:269a9d669b48 21
professorrodriguezse 1:269a9d669b48 22 Timer t;
professorrodriguezse 1:269a9d669b48 23
professorrodriguezse 1:269a9d669b48 24 void bno_init(void)
professorrodriguezse 1:269a9d669b48 25 {
professorrodriguezse 1:269a9d669b48 26 if(bno.check()) {
professorrodriguezse 1:269a9d669b48 27 pc.printf("BNO055 connected\r\n");
professorrodriguezse 1:269a9d669b48 28 bno.setmode(OPERATION_MODE_CONFIG);
professorrodriguezse 1:269a9d669b48 29 bno.SetExternalCrystal(1);
professorrodriguezse 1:269a9d669b48 30 //bno.set_orientation(1);
professorrodriguezse 1:269a9d669b48 31 bno.setmode(OPERATION_MODE_NDOF); //Uses magnetometer
professorrodriguezse 1:269a9d669b48 32 //bno.setmode(OPERATION_MODE_NDOF_FMC_OFF); //no magnetometer
professorrodriguezse 1:269a9d669b48 33 bno.set_angle_units(DEGREES);
professorrodriguezse 1:269a9d669b48 34 } else {
professorrodriguezse 1:269a9d669b48 35 pc.printf("BNO055 NOT connected\r\n Program Trap.");
professorrodriguezse 1:269a9d669b48 36 while(1);
professorrodriguezse 1:269a9d669b48 37 }
professorrodriguezse 1:269a9d669b48 38 }
professorrodriguezse 1:269a9d669b48 39
professorrodriguezse 1:269a9d669b48 40 float unwrap(float previous_angle, float new_angle)
professorrodriguezse 1:269a9d669b48 41 {
professorrodriguezse 1:269a9d669b48 42 float d = new_angle - previous_angle;
professorrodriguezse 1:269a9d669b48 43 //d = d > 180 ? d - 2 * 180 : (d < -180 ? d + 2 * 180 : d);
professorrodriguezse 1:269a9d669b48 44 if (d > 180) {
professorrodriguezse 1:269a9d669b48 45 d = d - 360;
professorrodriguezse 1:269a9d669b48 46 } else {
professorrodriguezse 1:269a9d669b48 47 if (d < -180) {
professorrodriguezse 1:269a9d669b48 48 d = d + 360;
professorrodriguezse 1:269a9d669b48 49 }
professorrodriguezse 1:269a9d669b48 50 }
professorrodriguezse 1:269a9d669b48 51 return previous_angle + d;
professorrodriguezse 1:269a9d669b48 52 }
professorrodriguezse 1:269a9d669b48 53
professorrodriguezse 1:269a9d669b48 54 int main()
professorrodriguezse 1:269a9d669b48 55 {
professorrodriguezse 1:269a9d669b48 56 servoOut1.pulseMin = 500;
professorrodriguezse 1:269a9d669b48 57 servoOut1.pulseMax = 2500;
professorrodriguezse 1:269a9d669b48 58 thread_sleep_for(50);
professorrodriguezse 1:269a9d669b48 59 float yaw, oldYaw;
professorrodriguezse 1:269a9d669b48 60 float currTime, dt;
professorrodriguezse 1:269a9d669b48 61 int controlSignal;
professorrodriguezse 1:269a9d669b48 62 float T = 17; //Period in ms
professorrodriguezse 1:269a9d669b48 63 t.start();
professorrodriguezse 1:269a9d669b48 64 pc.baud(115200);
professorrodriguezse 1:269a9d669b48 65 pc.printf("Starting Program... \n\r");
professorrodriguezse 1:269a9d669b48 66 bno_init();
professorrodriguezse 1:269a9d669b48 67 //can3.reset(); // reset the can bus interface
professorrodriguezse 1:269a9d669b48 68 can3.frequency(500000); // set up for 500K baudrate
professorrodriguezse 1:269a9d669b48 69 char msg_send[8];
professorrodriguezse 1:269a9d669b48 70 char msg_read_char[8];
professorrodriguezse 1:269a9d669b48 71 //servoOut1.pulse_us = 1500;
professorrodriguezse 1:269a9d669b48 72 bno.get_angles();
professorrodriguezse 1:269a9d669b48 73 yaw = bno.euler.yaw;
professorrodriguezse 1:269a9d669b48 74 pc.printf("Original Yaw: %.2f \r\n", yaw);
professorrodriguezse 1:269a9d669b48 75 thread_sleep_for(1000);
professorrodriguezse 1:269a9d669b48 76
professorrodriguezse 1:269a9d669b48 77 //pc.printf("CAN MCP2515 test: %s\r\n", __FILE__);
professorrodriguezse 1:269a9d669b48 78 while(1) {
professorrodriguezse 1:269a9d669b48 79 currTime = t.read();
professorrodriguezse 1:269a9d669b48 80 controlSignal = 1500 + (int)1000*(sin(currTime*2*3.14/10))*sin(currTime*2*3.14/26)*sin(currTime*2*3.14/26);
professorrodriguezse 1:269a9d669b48 81 servoOut1 = controlSignal;
professorrodriguezse 1:269a9d669b48 82 oldYaw = yaw;
professorrodriguezse 1:269a9d669b48 83 bno.get_angles();
professorrodriguezse 1:269a9d669b48 84 yaw = unwrap(oldYaw,bno.euler.yaw);
professorrodriguezse 1:269a9d669b48 85 //pc.printf("MyID: %d Raw Yaw Value: %.2f Unwrapped Yaw %.2f \r\n", myID, bno.euler.yaw, yaw);
professorrodriguezse 1:269a9d669b48 86
professorrodriguezse 1:269a9d669b48 87 sprintf(msg_send, "%.1f\r\n", yaw);
professorrodriguezse 1:269a9d669b48 88 for(int i=0; i<8; i++) {
professorrodriguezse 1:269a9d669b48 89 canTx_msg.data[i] = msg_send[i];
professorrodriguezse 1:269a9d669b48 90 }
professorrodriguezse 1:269a9d669b48 91 canTx_msg.id = myID;
professorrodriguezse 1:269a9d669b48 92 can3.write(&canTx_msg);
professorrodriguezse 1:269a9d669b48 93 pc.printf("Time: %.3f \t Angle: %.1f \t Control: %d\r\n", t.read(), yaw, controlSignal);
professorrodriguezse 1:269a9d669b48 94 dt = T-(t.read()-currTime)*1000;
professorrodriguezse 1:269a9d669b48 95 if (dt > 0) {
professorrodriguezse 1:269a9d669b48 96 thread_sleep_for(dt);
professorrodriguezse 1:269a9d669b48 97 }
professorrodriguezse 1:269a9d669b48 98
professorrodriguezse 1:269a9d669b48 99 }//while(1)
professorrodriguezse 1:269a9d669b48 100 }//main