Diff: main_old.txt

Revision:

1:38cd433ff221

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main_old.txt	Sun Sep 20 00:58:37 2015 +0000
@@ -0,0 +1,488 @@
+#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);
+    }
+}