File content as of revision 0:189488114aaa:

#include "mbed.h"
#include "ServoOut.h"
#include    "BNO055.h"

I2C    i2c(p28, p27);   // SDA, SCL
BNO055 imu(i2c, p29);
ServoOut leftmotor(p20);
ServoOut rightmotor (p19);


//  RAM -------------------------------------------------------------------------------------------
BNO055_ID_INF_TypeDef       bno055_id_inf;
BNO055_EULER_TypeDef        euler_angles;
BNO055_QUATERNION_TypeDef   quaternion;
BNO055_LIN_ACC_TypeDef      linear_acc;
BNO055_GRAVITY_TypeDef      gravity;
BNO055_TEMPERATURE_TypeDef  chip_temp;

//---------- Gyroscope Caliblation ------------------------------------------------------------
// (a) Place the device in a single stable position for a period of few seconds to allow the

DigitalOut led(LED1);
Timer tim; //Outputs time during each trial
float fl; //Force of left motor
float fr; //Force of right motor
float old_Mz=0; //Initializes previous torque value
float old_e=0; //Initializes previous error value
float e=0; //Initializes error value
float Mz=0; //Initializes torque value
float psi; //Computes actual heading
float psi_d; //Compues desired heading
float L; //Gain value
float angle_des; //change this based on what desired angle is
float t_s; //Time
float pi=3.14159;
float vL; //Left voltage
float vR; //Right voltage
float bias; //Addes extra force to move boat forward


void direction_ctrl();
void motor_ctrl();

int main()
{
    tim.start();
    while(tim.read()<45.0) {
        direction_ctrl(); //read direction of boath through imu
        motor_ctrl(); //implement direction at with direction desired to calculate motor power values and power motors with lbf values
        wait(0.01);
    }
    leftmotor = 0;
    rightmotor = 0;
    
    // Trap
    while(1);
}

//end of main
//start of void
void direction_ctrl()
{
    // end of calibration, start of reading the yaw, pitch and roll
    imu.get_Euler_Angles(&euler_angles);
    psi=euler_angles.h*(pi/180.0); //convert degrees to radians for actual position
    // eular_angles.h is yaw which is psi
}


void motor_ctrl()
{
    // running controller with variable aging
    if (tim.read()<=5 ) { //segment turns boat to desired heading, goes forward, turns around, and comes back all in five second incriments
        angle_des = 80;
        bias = 0;
    } else if(tim.read()>5 && tim.read()<=10) {
        bias = 5; //Increase voltage to move forward
    } else if(tim.read()>10 && tim.read()<=15+10) {
        angle_des = 260;
        bias = 0;
    } else if (tim.read()>15+10 && tim.read()<= 20+10) {
        bias = 5;
    }
//Sample time (NOT SETTLE TIME)
    t_s=0.01;

//calculate error
    psi_d=angle_des*pi/180.0; //find desired angle in radians
    e=psi_d-psi; //find difference between desired and actual angle
    Mz= 6*(0.9608*old_Mz + 32.56*e - 32.53*old_e); //Torque Control
    L=(24.0/12.0)*0.3048; //in to ft to m
    fl=Mz/(2.0*L)+bias; //convert torque to force
    fr=-Mz/(2.0*L)+bias; //convert torque to force

    vL=.019*(fl*fl*fl)-.4*(fl*fl)+3.3*fl-1.6; //convert force to voltage with the voltage to Newtons conversion for motor (/133 for boat motors)
    vR=.019*(fr*fr*fr)-.4*(fr*fr)+3.3*fr-1.6; //convert force to voltage with the voltage to Newtons conversion for motor

    leftmotor=1500.0+((500.0/12.0)*vL) ; // convert from voltage to proper pulsewidth
    rightmotor= 1500.0 + ((500.0/12.0)*vR); //convert from voltage to proper pulsewidth, 2000 pulsewidth gotten at 12 V

    // printf("angle=%+6.1f vL=%f vR=%f\r\n", euler_angles.h, vL, vR);
//Age variables
    old_e=e;
    old_Mz=Mz;
}