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