File content as of revision 0:6e9da949a2a3:

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

Serial pc(USBTX, USBRX);
I2C i2c(p9,p10);
DigitalOut pwr_on(p30);
BNO055 imu(i2c,p26);
//DigitalOut led1(LED1);

//instantiate global ready to 0
uint32_t ready = 0x00000000;

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;

// Instantiate motor driver PWM pins, duty cycle, and period
double stop_pw=.0015;
//double fwd_pw=.0018;
//double rev_pw=.0012;
PwmOut RH_motor(p23);
PwmOut Lfwd(p21); // left forward
PwmOut Lrev(p22); // left reverse
PwmOut Rfwd(p24); // right forward
PwmOut Rrev(p25); // right reverse
float dutycycle=0.40;


//Lazy... don't want to track these down everywhere
    //Standard servo rate -> 50Hz
double servo_freq=50;
double servo_period=(1/servo_freq);
    //25us per data sheet Blue Robotics BESC-R1
double esc_step=(25/1000000);
    //400Hz update frequency per data sheet
int esc_freq=400;
double esc_time_ms=(1000/esc_freq);
    //1.1-1.9ms pw per data sheet
double esc_range=.0004;
    //natural #
double e=2.71828;




//Data function pulls data from BNO and sends over serial
//Accepts unsigned 32bit key word (k) that RasPi will use as trigger
uint16_t data(uint32_t k) {
    //Instantiate status array of 6 32-bit words.
    //First 16 bits of each 32-bit word are Identifiers for RasPi to correctly assign the trailing 16 bits of data.
    uint32_t status[6]={0};

    //word 0: Key
        //Used to ensure Pi and Mbed are on same page.
    status[0]=k;

    //word 1: Status information.
        //0xffff acts as prefix to identify Status for RasPi.
        //Last 3 bits (from right) are current position (POS[0-7]). See BNO datasheet.
        //4th bit (from right) is RH turn motors enabled.
        //5th bit (from right) is LH turn motors enabled.
    status[1]=ready;

    //word 2: Calibration.
        //0xc000 acts as prefix to identify Cal for RasPi.
    status[2]=0xc0000000+imu.read_calib_status();

    //Get Euler data from BNO.
    imu.get_Euler_Angles(&euler_angles);

    //word 3 is Heading.
        //0xc100 acts as prefix to identify Heading for RasPi. 
    uint16_t h = euler_angles.h;
    status[3]=0xc1000000+h;

    //Offset calculation: 360*16bit resolution = 5760 -> converted to hex = 0x1680
    int offset=0x1680;

    //word 4 is Roll.
        //0xc300 acts as prefix to identify Roll for RasPi.
        //BNO sends Roll and Pitch as +/- 180deg. Add offset of 360deg to avoid dealing with signed ints over serial.
    uint16_t r = offset + euler_angles.r;
    status[4]=0xc3000000+r;
    
    //word 5 is Pitch.
        //0xc500 acts as prefix to identify Pitch for RasPi.
        //BNO sends Roll and Pitch as +/- 180deg. Add offset of 360deg to avoid dealing with signed ints over serial.
    uint16_t p = offset + euler_angles.p;
    status[5]=0xc5000000+p;

    //For loop iterates through Status array to transmit 6 32-bit words over Serial with "\n" appended for Python in RasPi.
    int i;
    int l=(sizeof(status)/sizeof(uint32_t))-1;
    for (i=0; i<=l; i++) {
        pc.printf("%x\n", status[i]);
  }
 
  //return heading
  return h;
}

//Function to setup motor PWM for right turn
void TurnRight() {
    //mask off other bits and set Rmotor status to on
    ready=(ready & 0xfffffff7) + 0x8;
//    pc.printf("Turning Right\r\n");
    Rfwd.write(dutycycle);
    Lrev.write(dutycycle);
}

//Function to setup motor PWM for left turn
void TurnLeft() {
    //mask off other bits and set Lmotor status to on
    ready=(ready & 0xffffffef) + 0x10;
//  pc.printf("Turning Left\r\n");
    Lfwd.write(dutycycle);
    Rrev.write(dutycycle);
}

//Function to stop all motors
void Stop_motors() {
    //Set PW on all PWM pins to 0
    Lfwd.write(0);
    Lrev.write(0);
    Rfwd.write(0);
    Rrev.write(0);
    //zero out Lm and Rm status bits
    ready=(ready & 0xffffffe7);
}

//The sigmoid curve for logistic function.
//Used for continuous PWM and softstart / softstop near end points.
double Sigmoid(double Ymax, double m, double x0, double x) {
//  double Logistic_func=(Ymax/(1+(e^((-1*m)*(x-x0)))));
    double exponent=((-1*m)*(x-x0));
    double power=pow(e,exponent);
    double Logistic_func=Ymax/(1+power);
//  pc.printf("exp: %f, power:%f, ret: %f\r\n",exponent,power,Logistic_func);
    return Logistic_func;
}

//Function to control RH motor
void RH_motor_cntrl(double target_pw) {
    double step_pw=.00005;
    double current_pw=(RH_motor.read()*.02);
    double diff_pw=abs(target_pw-current_pw);
    //Time we want to take to get from 1.5 to [1.9 or 1.1]
//  double Delay=0.5; //500ms
    //Total # of steps / resolution of ESC
//  double Steps=((.0019-.0015)/esc_step);
    //Center of sigmoid curve
    double Center=.1; //100ms -> ramp up within 200ms
    //slope at Center
    double Slope=50; //rate of ramp up
    double time_count=0; //total time in while loop
    double dwell_time=0.001; //time to wait between while loop
    double tolerance_pw=.000045; //45us
    double dir=0;
    while(diff_pw>tolerance_pw) {
        current_pw=(RH_motor.read()*.02);
        diff_pw=abs(target_pw-current_pw);
        if ((target_pw-current_pw)>=0) {
            dir=(1);
            //RH moving fwd
//            ready=(ready & 0xfffffff7) + 0x8;
        }
        if ((target_pw-current_pw)<0) {
            dir=(-1);
            //RH moving rev
//            ready=(ready & 0xffffffef) + 0x10;
        }
        double factor=Sigmoid(1,Slope,Center,time_count);
        //double new_pw=stop_pw+(diff_pw*factor);
        double new_pw;
        double speed_pw=step_pw+(diff_pw*factor);
        if (dir==1) new_pw=current_pw+speed_pw;
            //new_pw=current_pw+.0000025;
        if (dir==-1) new_pw=current_pw-speed_pw;
            //new_pw=current_pw-.0000025;
        pc.printf("diff: %f, factor: %f\r\n",diff_pw,factor);
        pc.printf("pw: %f, speed: %f, offset: %f\r\n",current_pw,speed_pw,step_pw);
        RH_motor.pulsewidth(new_pw);
        wait_ms(dwell_time);
        time_count+=dwell_time;
    }
    RH_motor.pulsewidth(target_pw);
    //zero out Lm and Rm status bits
//    ready=(ready & 0xffffffe7);
}

//Function to handle motor logic
void motors(int desired_heading) {
    //Pull heading from BNO and correct for 16bit/deg resolution.
    double current_heading=((data(0x1234abcd))/0x10);
    //Set acceptable tolerance in deg, for heading accuracy.
    double tolerance = 3.0;
    //The natural #

    //Error handling. Only continue for 0-360 range.
    if (desired_heading>=0 and desired_heading<=360) {
        //Calculate how far to turn in degrees.
        double diff = abs(desired_heading-(current_heading));
        //Correct for 360-0 edge cases if 'diff'erence is greater than 180.
        //Change direction and recalculate for accurate 'tolerance' comparison.
        if (diff>180) {
            if (desired_heading>180) diff=((current_heading+180)-(desired_heading-180));
            if (current_heading>180) diff=((desired_heading+180)-(current_heading-180));
        }
        while (diff > tolerance) {
            double dir=1;
            double speed=0;
            current_heading=((data(0x1234abcd))/0x10);
            diff=abs((desired_heading)-(current_heading));
            //Sigmoid data
            double center=15; //deg
            double slope=0.5; //rate of change
            if (diff>180) {
                dir=(-1);
                if (desired_heading>180) diff=((current_heading+180)-(desired_heading-180));
                if (current_heading>180) diff=((desired_heading+180)-(current_heading-180));
            }
            if ((desired_heading-current_heading)<0) dir=(dir*-1);
            speed=Sigmoid(esc_range,slope,center,diff);
            if (dir==1) {
                RH_motor_cntrl(stop_pw+speed);
                TurnRight();
            }
            if (dir==-1) {
                RH_motor_cntrl(stop_pw-speed);
                TurnLeft();
            }
            wait_ms(20);
        }
//      pc.printf("heading: %i, diff: %i\r\n",current_heading,diff);
    }
}

void maintain(int desired_heading) {
    while(1) {
        motors(desired_heading);
        wait_ms(20);
    }
}

//Function to accept desired position, program BNO,
//  and update ready word by masking non-position bits
//  and adding the new position to position bits
void switch_pos(int position) {
    if (position>=0 and position<8) {
        switch (position) {
            case 1:
                imu.set_mounting_position(MT_P1);
                ready=((ready & 0xfffffff8)+0x1);
                break;
            case 2:
                imu.set_mounting_position(MT_P2);
                ready=((ready & 0xfffffff8)+0x2);
                break;
            case 3:
                imu.set_mounting_position(MT_P3);
                ready=((ready & 0xfffffff8)+0x3);
                break;
            case 4:
                imu.set_mounting_position(MT_P4);
                ready=((ready & 0xfffffff8)+0x4);
                break;
            case 5:
                imu.set_mounting_position(MT_P5);
                ready=((ready & 0xfffffff8)+0x5);
                break;
            case 6:
                imu.set_mounting_position(MT_P6);
                ready=((ready & 0xfffffff8)+0x6);
                break;
            case 7:
                imu.set_mounting_position(MT_P7);
                ready=((ready & 0xfffffff8)+0x7);
                break;
            case 0:
            default:
                imu.set_mounting_position(MT_P0);
                ready=((ready & 0xfffffff8));
                break;
        }
    }
}

//Commands function handles Serial input, checks for correct syntax, and calls appropriate function(s) to execute commands.
void commands() {
    //Instantiate input char array to buffer Serial input.
    char input[1000];
    //check_pos char array used to filter Position commands.
    char check_pos[5]="pos:";
    //check_hea char array used to filter Go to Heading commands.
    char check_hea[5]="hea:";
    //check_mnt char array used to filter Maintain Heading commands.
    char check_mnt[5]="mnt:";

    //While loop reads Serial input into input buffer.
    int i=0;
    while (pc.readable()) {
        input[i]=pc.getc();
//        pc.printf("read char %c to %i\r\n",input[i], i);
        i++;
    }

    //Only continue if input buffer has 7 entries, otherwise ignore input buffer. 
    //All commands from RasPi shall come in a format of 7 characters "abc:xyz" 
    //      where 'abc' is an identifying string and 'xyz' is a decimal number.
    if (i==7) {
//        pc.printf("checking 0-%c, 1-%c, 2-%c, 3-%c, 4-%c \r\n",check_pos[0],check_pos[1],check_pos[2],check_pos[3],check_pos[4]);

        //Instantiate counters that will be used to verify known commands.
        int verified_pos=0;
        int verified_hea=0;
        int verified_mnt=0;
        
        //While loop checks first 4 characters of input buffer and attempts to match
        //      against known commands.
        int q=0;
        while (q<4) {
            //Increment verified_pos if a match is found between Serial input buffer
            //      and Position command format.
            if (input[q]==check_pos[q]) {
                verified_pos++;
//                pc.printf("verified for pos %c at %i\r\n",input[q],q);
            }
            
            //Increment verified_hea if a match is found between Serial input buffer
            //      and Heading command format.
            if (input[q]==check_hea[q]) {
//                pc.printf("verified for hea %c at %i\r\n",input[q],q);
                verified_hea++;
            }
            
            if (input[q]==check_mnt[q]) {
                verified_mnt++;
            }
            q++;
        }
        
        //If first 4 characters from Serial input buffer match Position command format,
        //      execute "switch_pos" function.
        if (verified_pos==4) {
            int change=(input[6]-'0');
            switch_pos(change);
        }
        
        //If first 4 characters from Serial input buffer match Heading command format,
        //      execute "motors" function.
        if (verified_hea==4) {
            //Correct for ascii '0', and reform 3digit decimal number
            int hea100=(input[4]-'0');
            int hea10=(input[5]-'0');
            int hea1=(input[6]-'0');
            int hea=(hea100*100)+(hea10*10)+(hea1*1);
//            pc.printf("heading: %i\r\n",hea);
            motors(hea);
            //zero out ready status for LH and RH motors
            ready=(ready & 0xffffffe7);
            Stop_motors();
            RH_motor_cntrl(stop_pw);
        }
        
        if (verified_mnt==4) {
            int mnt100=(input[4]-'0');
            int mnt10=(input[5]-'0');
            int mnt1=(input[6]-'0');
            int mnt=(mnt100*100)+(mnt10*10)+(mnt1*1);
            maintain(mnt);
            RH_motor_cntrl(stop_pw);
            //zero out ready status for LH and RH motors
            ready=(ready & 0xffffffe7);
        }
    }
}

int main() {
    //engage plaidspeed
    pc.baud(115200);
//    imu.set_mounting_position(MT_P3);
    RH_motor.period(.02);
    RH_motor.pulsewidth(stop_pw);

    //If not ready, reset BNO.
    while (imu.chip_ready() == 0) {
        do {
            pc.printf("resetting BNO\r\n");
            pwr_on=0;
            wait_ms(100);
            pwr_on=1;
            wait_ms(50);
        } while(imu.reset());
    }
    wait_ms(20);

    //If BNO is ready, set ready status indication
    if (imu.chip_ready()) {
        ready=0xffff0000;
    }
//    uint32_t keyver=intake(ready);

    //Look for serial input commands and send to 'commands' function.
    //If no serial input commands, stream data.
    while(1) {
        if (pc.readable()) commands();
        else data(0x1234abcd);
        wait_ms(20);
    }
}