File content as of revision 2:e585627bae99:

#include "mbed.h"
#include "USBMouseKeyboard.h"
#include "DebounceIn.h"

//LEDs to indicate clibration status
DigitalOut redLED(LED_RED);
DigitalOut greenLED(LED_GREEN);

//pushbuttons inputs
DebounceIn trigger(D3);
DebounceIn move(D4);
DebounceIn door(D5);
DebounceIn centerBut(D12);

// Motor output
DigitalOut vibrate(D2);
Timeout vibrateTimeout;
 
//USBMouseKeyboard
//do we need absolute or relative mouse?
USBMouseKeyboard key_mouse(ABS_MOUSE);
HID_REPORT kbHIDReport;;

Serial pc(USBTX,USBRX);
I2C i2c(D7, D6);

const int bno055_addr = 0x28 << 1;

const int BNO055_ID_ADDR                                          = 0x00;
const int BNO055_EULER_H_LSB_ADDR                                 = 0x1A;
const int BNO055_GRAVITY_DATA_X_LSB_ADDR                          = 0x2E;
const int BNO055_TEMP_ADDR                                        = 0x34;
const int BNO055_OPR_MODE_ADDR                                    = 0x3D;
const int BNO055_CALIB_STAT_ADDR                                  = 0x35;
const int BNO055_SYS_STAT_ADDR                                    = 0x39;
const int BNO055_SYS_ERR_ADDR                                     = 0x3A;
const int BNO055_AXIS_MAP_CONFIG_ADDR                             = 0x41;
const int BNO055_SYS_TRIGGER_ADDR                                 = 0x3F;
const int BNO055_ACC_DATA_X_ADDR                                  = 0x08;

typedef struct CalibStatus_t
{
    int mag;
    int acc;
    int gyr;
    int sys;
} CalibStatus;

typedef struct Euler_t
{
    float heading;
    float pitch;
    float roll;
} Euler;

// The "zero" offset positions
short int headingOffset;
short int pitchOffset;
short int rollOffset;


/**
 * Function to write to a single 8-bit register
 */
void writeReg(int regAddr, char value)
{
    char wbuf[2];
    wbuf[0] = regAddr;
    wbuf[1] = value;
    i2c.write(bno055_addr, wbuf, 2, false);  
}

/**
 * Function to read from a single 8-bit register
 */
char readReg(int regAddr)
{
    char rwbuf = regAddr;
    i2c.write(bno055_addr, &rwbuf, 1, false);
    i2c.read(bno055_addr, &rwbuf, 1, false);
    return rwbuf;
}

/**
 * Returns the calibration status of each component
 */
CalibStatus readCalibrationStatus()
{
    CalibStatus status;
    int regVal = readReg(BNO055_CALIB_STAT_ADDR);
        
    status.mag = regVal & 0x03;
    status.acc = (regVal >> 2) & 0x03;
    status.gyr = (regVal >> 4) & 0x03;
    status.sys = (regVal >> 6) & 0x03;
    
    return status;
}


/**
 * Returns true if all the devices are calibrated
 */
bool calibrated()
{
    CalibStatus status = readCalibrationStatus();
    
    if(status.mag == 3 && status.acc == 3 && status.gyr == 3)
        return true;
    else
        return false;
}
  
 
/**
 * Checks that there are no errors on the accelerometer
 */
bool bno055Healthy()
{
    int sys_error = readReg(BNO055_SYS_ERR_ADDR);
    wait(0.001);
    int sys_stat = readReg(BNO055_SYS_STAT_ADDR);
    wait(0.001);
    
    if(sys_error == 0 && sys_stat == 5)
        return true;
    else {
        //pc.printf("SYS_ERR: %d SYS_STAT: %d\r\n", sys_error, sys_stat);
        return false;
    }
}
    

/**
 * Configure and initialize the BNO055
 */
bool initBNO055()
{
    unsigned char regVal;
    i2c.frequency(400000);
    bool startupPass = true;
    
    // Do some basic power-up tests
    regVal = readReg(BNO055_ID_ADDR);
    if(regVal == 0xA0)
        pc.printf("BNO055 successfully detected!\r\n");
    else {
        pc.printf("ERROR: no BNO055 detected\r\n");
        startupPass = false;
    }
        
    regVal = readReg(BNO055_TEMP_ADDR);
    pc.printf("Chip temperature is: %d C\r\n", regVal);
    
    if(regVal == 0)
        startupPass = false;
 
    // Change mode to CONFIG
    writeReg(BNO055_OPR_MODE_ADDR, 0x00);
    wait(0.2);
    
    regVal = readReg(BNO055_OPR_MODE_ADDR);
    pc.printf("Change to mode: %d\r\n", regVal);
    wait(0.1);
    
    // Remap axes
    writeReg(BNO055_AXIS_MAP_CONFIG_ADDR, 0x06);    // b00_00_01_10
    wait(0.1);    

    // Set to external crystal
    writeReg(BNO055_SYS_TRIGGER_ADDR, 0x80);
    wait(0.2);    

    // Change mode to NDOF
    writeReg(BNO055_OPR_MODE_ADDR, 0x0C);
    wait(0.2);
 
    regVal = readReg(BNO055_OPR_MODE_ADDR);
    pc.printf("Change to mode: %d\r\n", regVal);
    wait(0.1);
    
    return startupPass;
}

/**
 * Sets the current accelerometer position as the zero position.
 */
void setZeroPosition()
{
    char buf[16];
    
    // Read the current euler angles and set them as the zero position
    buf[0] = BNO055_EULER_H_LSB_ADDR;
    i2c.write(bno055_addr, buf, 1, false);
    i2c.read(bno055_addr, buf, 6, false);
        
    headingOffset = buf[0] + (buf[1] << 8);
    rollOffset = buf[2] + (buf[3] << 8);
    pitchOffset = buf[4] + (buf[5] << 8);
}


/**
 * Sets the current accelerometer position as the zero position.
 */
void setZeroHeading()
{
    char buf[16];
    
    // Read the current euler angles and set them as the zero position
    buf[0] = BNO055_EULER_H_LSB_ADDR;
    i2c.write(bno055_addr, buf, 1, false);
    i2c.read(bno055_addr, buf, 6, false);
        
    headingOffset = buf[0] + (buf[1] << 8);
}


/**
 * Reads the Euler angles, zeroed out
 */
Euler getEulerAngles()
{
    char buf[16];
    Euler e;
    
    // Read in the Euler angles
    buf[0] = BNO055_EULER_H_LSB_ADDR;
    i2c.write(bno055_addr, buf, 1, false);
    i2c.read(bno055_addr, buf, 6, false);
    
    short int euler_head = buf[0] + (buf[1] << 8);
    short int euler_roll = buf[2] + (buf[3] << 8);
    short int euler_pitch = buf[4] + (buf[5] << 8);
    
    e.heading = ((int)euler_head - (int)headingOffset) / 16.0;
    e.roll = ((int)euler_roll - (int)rollOffset) / 16.0;
    e.pitch = ((int)euler_pitch - (int)pitchOffset) / 16.0;
    
    if(e.pitch > 90 || e.pitch < -90)
        e.pitch = 0;
    
    return e;
}


/***** Functions to vibrate the motor (non-blocking call) *****/
void vibrateOff()
{
    vibrate = 0;
}

void vibrateMotor()
{
    vibrate = 1;
    vibrateTimeout.attach(&vibrateOff, 0.1);
}




int main() {
    
    uint16_t x_center = (X_MAX_ABS - X_MIN_ABS)/2;
    uint16_t y_center = (Y_MAX_ABS - Y_MIN_ABS)/2;
    uint16_t x_screen = 0;
    uint16_t y_screen = 0;
    
    int loop_counter = 0;
    int shake_counter = 0;
    
   
    //uint32_t x_origin = x_center;
    //uint32_t y_origin = y_center;
    //uint32_t radius = 5000;
    //uint32_t angle = 0;
    
    redLED = 0;
    bool startupPassed;
    Euler e;
    bool down;
    CalibStatus calStat;

    // Record old state of buttons
    bool triggerPressed = false;
    bool movePressed = false;

    // USB HID report to send move up/down
    kbHIDReport.length = 4;
    kbHIDReport.data[0] = 1;        // USB ID
    kbHIDReport.data[1] = 0;        // modifier key
    kbHIDReport.data[2] = 0;        // don't know

    // Initialize
    pc.baud(115200);
    trigger.mode(PullUp);
    move.mode(PullUp);
    door.mode(PullUp);
    centerBut.mode(PullUp);
    wait(0.8);
    startupPassed = initBNO055();   // Note: set LED to RED if this fails
    
    // Wait until calibration passes
    
    while(!calibrated()){
        wait(0.1);
        calStat = readCalibrationStatus();
        printf("MAG: %d ACC: %d GYR: %d SYS: %d\r\n", calStat.mag, calStat.acc, calStat.gyr, calStat.sys);      
        wait(0.5); 
    }
    redLED = 1;
    greenLED = 0;   
     
    pc.printf("Board fully calibrated!\r\n");
    
    // Wait until user hits the trigger. Then zero out the readings
    while(trigger.read() == 1) {
        wait(0.01);
    }
    setZeroPosition();

    // Read orientation values
    while(true)
    {
        // Make sure that there are no errors
        if(!bno055Healthy())
        {
            //wait(0.1);
            //calStat = readCalibrationStatus();
            //pc.printf("Heading: %7.2f \tRoll: %7.2f \tPitch: %7.2f Down: %d \tMAG: %d ACC: %d GYR: %d SYS: %d\r\n", e.heading, e.roll, e.pitch,down, calStat.mag, calStat.acc, calStat.gyr, calStat.sys); 
            pc.printf("ERROR: BNO055 has an error/status problem!!!\r\n");
        }
        
        // Read in the Euler angles
        e = getEulerAngles();
        wait(0.001);
 
        // Read in the calibration status
        calStat = readCalibrationStatus();
        wait(0.001);
        
        // LED red if not calibrated else green
        if(!calibrated())
        {
            redLED = 0;
            greenLED = 1;
        }
        else
        {
            redLED = 1;
            greenLED = 0;
        }
            
/*        
        // Check if device is pointing down
        //down = (e.pitch < -70);
        
        wait(0.01);
        char buf[16];
        buf[0] = BNO055_ACC_DATA_X_ADDR;
        i2c.write(bno055_addr, buf, 1, false);
        i2c.read(bno055_addr, buf, 6, false);
        
        short int acc_z = buf[4] + (buf[5] << 8);
        wait(0.01);
        
        if(acc_z < -1000 || acc_z > 1000)
        {
            shake_counter++;
            loop_counter++;
        }
        else if (loop_counter > 0)
            loop_counter ++;
            
        if(shake_counter > 2 && loop_counter < 20) {
             key_mouse.scroll(-1);
             wait(1.5);
             shake_counter = 0;
             loop_counter = 0;
        }
        else if(loop_counter == 20) {
           shake_counter = 0;
           loop_counter = 0;
        }
*/           
/*
        //if it is down, then change device
        if(down){
            key_mouse.scroll(-1);
            
            // Wait until up
            do {
                wait(0.01);
                e = getEulerAngles();
                wait(0.01);
            } while(e.pitch < -20);

            wait(2);
            
            // Re-center heading
            //setZeroHeading();
            //wait(0.01);
        }
*/        
        // If trigger state changed
        if (!trigger.read() != triggerPressed)
        {
            // If trigger pressed
            if(!trigger.read())
            {
                //key_mouse.click(MOUSE_LEFT);
                //key_mouse.keyCode('d');
                kbHIDReport.data[3] = 0x07;     // UP arrow
                vibrateMotor();
                triggerPressed = true;
            }
            
            else {
                triggerPressed = false;
                kbHIDReport.data[3] = 0x00;     // UP arrow
            }
            key_mouse.sendNB(&kbHIDReport);
        }

        if (!door.read()){
            int counter = 0;
            while(counter<50){
                key_mouse.keyCode(' ');
                counter++;
                wait(0.001);
            }
            wait(0.2);
        }
        
        if(!centerBut.read())
        {
            wait(0.01);
            setZeroPosition();
            wait(0.1);
        }

        // If move button state changed
        if (!move.read() != movePressed)
        {
            // If move pressed
            if(!move.read()) {
                kbHIDReport.data[3] = 0x52;     // UP arrow
                movePressed = true;
            }
            
            else {
                kbHIDReport.data[3] = 0x00;     // no press
                movePressed = false;
            }
            
            key_mouse.sendNB(&kbHIDReport);
        }
  

        // move limits
        float heading_limited;
        if(e.heading > 90)
            heading_limited = 90;
        else if(e.heading < -90)
            heading_limited = -90;
        else if(e.heading < 2 && e.heading > -2)
            heading_limited = 0;
        //else if (e.pitch < -30)
        //    heading_limited = 0;
        else if(e.heading > -20 && e.heading < 20)
            heading_limited = 16 * e.heading;
        else
            heading_limited = 0.8 * e.heading * abs(e.heading);

 

        //moving the mouse now
        x_screen = x_center + heading_limited;
        //x_screen = x_center+(e.heading/180*(X_MAX_ABS-x_center)); //45 was too sensitive
        y_screen = y_center;
        
        printf("Heading: %7.2f \tRoll: %7.2f \tPitch: %7.2f Down: %d \tMAG: %d ACC: %d GYR: %d SYS: %d\r\n", e.heading, e.roll, e.pitch,
            down, calStat.mag, calStat.acc, calStat.gyr, calStat.sys);
            
        key_mouse.move(x_screen, y_screen);
        wait(0.02);
    }
}