Diff: main.cpp

Revision:

1:38cd433ff221

Parent:

0:a59b1f819776

Child:

2:e585627bae99

--- a/main.cpp	Fri Sep 18 00:40:26 2015 +0000
+++ b/main.cpp	Sun Sep 20 00:58:37 2015 +0000
@@ -1,21 +1,30 @@
 #include "mbed.h"
 #include "USBMouseKeyboard.h"
+#include "DebounceIn.h"
+
 //LEDs to indicate clibration status
 DigitalOut redLED(LED_RED);
 DigitalOut greenLED(LED_GREEN);
 
 //pushbuttons inputs
-DigitalIn trigger(D11);
-DigitalIn move(D8);
-DigitalIn door(D4);
+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;;
 
+// Communication busse
 Serial pc(USBTX,USBRX);
 I2C i2c(D7, D6);
 
+// Constant declarations for the accelerometer chip
 const int bno055_addr = 0x28 << 1;
 
 const int BNO055_ID_ADDR                                          = 0x00;
@@ -28,6 +37,7 @@
 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
 {
@@ -192,6 +202,22 @@
 
 
 /**
+ * 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()
@@ -212,40 +238,65 @@
     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);
+}
+
+
+/**
+ * The main function
+ */
 int main() {
     
+    // Declare variables
     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;
-   
-    //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
+    redLED = 0;
     
     // Wait until calibration passes
-    
     while(!calibrated()){
         wait(0.1);
-        CalibStatus calStat = readCalibrationStatus();
+        calStat = readCalibrationStatus();
         printf("MAG: %d ACC: %d GYR: %d SYS: %d\r\n", calStat.mag, calStat.acc, calStat.gyr, calStat.sys);      
         wait(0.5); 
     }
@@ -255,7 +306,7 @@
     pc.printf("Board fully calibrated!\r\n");
     
     // Wait until user hits the trigger. Then zero out the readings
-    while(trigger == 1) {
+    while(trigger.read() == 1) {
         wait(0.01);
     }
     setZeroPosition();
@@ -265,10 +316,12 @@
     {
         // Make sure that there are no errors
         if(!bno055Healthy())
-            wait(0.1);
-            CalibStatus 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); 
+        {
+            //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();
@@ -278,42 +331,93 @@
         calStat = readCalibrationStatus();
         wait(0.001);
         
-        // Check if device is pointing down
-        down = (e.pitch < -70);
-        
-        //if it is down, then change device
-        if (down){
-            key_mouse.click(MOUSE_RIGHT);
-            wait(0.3); //do we need to change the wait time
+        // LED red if not calibrated else green
+        if(!calibrated())
+        {
+            redLED = 0;
+            greenLED = 1;
+        }
+        else
+        {
+            redLED = 1;
+            greenLED = 0;
         }
-        if (!trigger){
-            key_mouse.click(MOUSE_LEFT);
-            wait(0.05); //0.3 was too long
-        }                                                   
-        if (!door){
+ 
+        // If trigger state changed
+        if (!trigger.read() != triggerPressed)
+        {
+            // If trigger pressed
+            if(!trigger.read())
+            {
+                kbHIDReport.data[3] = 0x07;     // D key press
+                vibrateMotor();
+                triggerPressed = true;
+            }
+            
+            else {
+                triggerPressed = false;
+                kbHIDReport.data[3] = 0x00;     // UP arrow
+            }
+            key_mouse.sendNB(&kbHIDReport);
+        }
+
+        // If the door open button was pressed
+        if (!door.read()){
             int counter = 0;
             while(counter<50){
                 key_mouse.keyCode(' ');
                 counter++;
+                wait(0.001);
             }
-            wait(0.5);
+            wait(0.2);
+        }
+        
+        // If the re-center button was pressed
+        if(!centerBut.read())
+        {
+            wait(0.01);
+            setZeroPosition();
+            wait(0.1);
         }
-        if (!move){
-            int count = 0;
-            while (count<100){
-                key_mouse.keyCode('a');  
-                count++;
+
+        // 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.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+(e.heading/180*(X_MAX_ABS-x_center)); //45 was too sensitive
+        x_screen = x_center + heading_limited;
         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);
-        
+        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.05);
+        wait(0.02);
     }
 }