Diff: main.cpp

Revision:

2:5fec7dd286d2

Parent:

1:8546d208bd4f

--- a/main.cpp	Wed Apr 27 16:41:29 2016 +0000
+++ b/main.cpp	Thu Apr 28 04:56:44 2016 +0000
@@ -3,146 +3,210 @@
 
 Serial pc(USBTX, USBRX);
 
-AnalogIn inputx(PTB0); // AnalogIn inputx(PTB0);
-AnalogIn inputy(PTB1); //AnalogIn inputy(PTB1);
-AnalogIn inputz(PTB2); //AnalogIn inputz(PTB2);
+int sumX; // Used for zero averaging
+int sumY; // Used for zero averaging
+int sumZ; // Used for zero averaging
+
+// Up/Down Accel //
+AnalogIn inputUDx(PTB0);
+AnalogIn inputUDy(PTB1);
+AnalogIn inputUDz(PTB2);
+
+int xUD1=0; // X-Offset
+int yUD1=0; // Y-Offset
+int zUD1=0; // Z-Offset
+
+int xUD=0; // variable for x axes
+int yUD=0; // variable for y axes
+int zUD=0; // variable for z axes
 
-Timeout debounceUp;
-Timeout debounceDown;
-Timeout middle1;
-Timeout middle2;
+// ----------- //
+
+// Left/Right Accel //
+AnalogIn inputLRx(PTB3);
+AnalogIn inputLRy(PTC2);
+AnalogIn inputLRz(PTC1);
 
+int xLR1=0; // X-Offset
+int yLR1=0; // Y-Offset
+int zLR1=0; // Z-Offset
 
+int xLR=0; // variable for x axes
+int yLR=0; // variable for y axes
+int zLR=0; // variable for z axes
+// ----------- //
+
+// Debugging LEDs //
 PwmOut led1(LED1);
 PwmOut led2(LED2);
 PwmOut led3(LED3);
-
-int xR; 
-int yR; 
-int zR;
+// ----------- //
 
-int sumX;
-int sumY;
-int sumZ;
- 
+//    ISRs     //
+Timeout debounceUp; 
+Timeout debounceDown;
+Timeout debounceLeft; 
+Timeout debounceRight;
 
-int x1=0; // variable for initialization
-int y1=0; // variable for initialization
-int z1=0; // variable for initialization
-
+Timeout middleUp;
+Timeout middleDown;
+Timeout middleLeft;
+Timeout middleRight;
 
-int x=0; // variable for x axes
-int y=0; // variable for y axes
-int z=0; // variable for z axes
+bool mFUp = 1;
+bool mFDown = 1;
+bool mFLeft = 1;
+bool mFRight = 1;
 
-
-bool midFl1 = 1;
-bool midFl2 = 1;
 bool dwn = 0;
 bool up = 0;
+bool lft = 0;
+bool rht = 0;
 
-void DBUp(){
-    up = 0;
-    }
+// Debounce ISRs //
+void DBUp(){up = 0;}
+
+void DBDown(){dwn = 0;}
+
+void DBLeft(){lft = 0;}
+
+void DBRight(){rht = 0;}
 
-void DBDown(){
-    dwn = 0;
-    }
-    
-void mid1(){
-    midFl1 = 1;
-    }
+//---------------//
+
+// Middle Flag ISRs//
+void midUp(){mFUp = 1;}
 
-void mid2(){
-    midFl2 = 1;
-    }
+void midDown(){mFDown = 1;}
 
+void midLeft(){mFLeft = 1;}
+
+void midRight(){mFRight = 1;}
+//---------------//
 
 int main() {
-//led1=0.9;     
-pc.baud(9600);   // baud rate: 9600 bps interaction with computer
-
-wait(1);
+    pc.baud(9600);
+    wait(1);
 
-for(int i =0; i < 10;i++){
-    xR = inputx.read_u16();
-    yR = inputy.read_u16();
-    zR = inputz.read_u16();
-    sumX += xR;
-    sumY += yR;
-    sumZ += zR;
+    for(int i =0; i < 10;i++){
+        xUD = inputUDx.read_u16();
+        yUD = inputUDy.read_u16();
+        zUD = inputUDz.read_u16();
+        sumX += xUD;
+        sumY += yUD;
+        sumZ += zUD;
 }
 
-x1 = sumX/10;
-y1 = sumY/10;
-z1 = sumZ/10;
+    xUD1 = sumX/10;
+    yUD1 = sumY/10;
+    zUD1 = sumZ/10;
+
+    sumX = 0; // Reset sum terms for LR controller
+    sumY = 0;
+    sumZ = 0;
 
-//************Eric Algorithm******************
-int status[]={0,0}; //0 means break,1 is up, 2 is down. status(0) is last, status(1) is current
-int L=1; // 0 = not outputting, =1 is ouputting
-//********************************************
+    for(int i =0; i < 10;i++){
+        xLR = inputLRx.read_u16();
+        yLR = inputLRy.read_u16();
+        zLR = inputLRz.read_u16();
+        sumX += xLR;
+        sumY += yLR;
+        sumZ += zLR;
+}
+
+    xLR1 = sumX/10;
+    yLR1 = sumY/10;
+    zLR1 = sumZ/10;
+
+    USBKeyboard keyboard;
+    while (true) {
+        // Up/Down Read
+        xUD = inputUDx.read_u16()-xUD1; 
+        yUD = inputUDy.read_u16()-yUD1;
+        zUD = inputUDz.read_u16()-zUD1;
+        
+        // Left/Right Read
+        xLR = inputLRx.read_u16()-xLR1; 
+        yLR = inputLRy.read_u16()-yLR1;
+        zLR = inputLRz.read_u16()-zLR1; 
 
 
-USBKeyboard keyboard;
-    while (true) {
-        L=1;
-        z = inputz.read_u16()-z1; //Calibration
-        x = inputx.read_u16()-x1; //Calibration
-        y = inputy.read_u16()-y1; //Calibration
-//        pc.printf("X: %d Y: %d Z: %d \n\r", x,y,z);
-         //Establish Up
-        if (x > -800 && y < 1500 && up == 0) { 
-//            status[0] = status[1];
-//            status[1] = 1;
-//            pc.printf("Down");
+        //Establish Up
+        if (xUD > -800 && yUD < 1500 && up == 0) { 
             keyboard.keyCode(UP_ARROW);
             led1 = 1;
             led2 = 0.9;
             led3 = 1;
-            
             up = 1;
-            
             debounceUp.attach(&DBDown, 0.100);
-//            up = 0;
-
+        }
+         
+        //Establish Down
+        if(xUD < -3500 && yUD > 5000 && dwn == 0) {
+            keyboard.keyCode(DOWN_ARROW);
+            led1 = 1;
+            led2 = 1;
+            led3 = 0.9;    
+            dwn = 1;
+            debounceDown.attach(&DBUp, 0.100);
         }
          
-         //Establish Down
-        if(x < -3500 && y > 5000 && dwn == 0) {
-//            status[0] = status[1];
-//            status[1] = 2;
-//                pc.printf("Up");
-                keyboard.keyCode(DOWN_ARROW);
-                led1 = 1;
-                led2 = 1;
-                led3 = 0.9;    
-                dwn = 1;
-                debounceDown.attach(&DBUp, 0.100);
-//                dwn = 0;
-                
+        //Establish Left
+        if (xLR > -800 && yLR < 1500 && lft == 0) { 
+            keyboard.keyCode(LEFT_ARROW);
+            led1 = 1;
+            led2 = 0.9;
+            led3 = 1;
+            lft = 1;
+            debounceRight.attach(&DBRight, 0.100);
         }
          
+        //Establish Right
+        if(xLR < -3500 && yLR > 5000 && rht == 0) {
+            keyboard.keyCode(RIGHT_ARROW);
+            led1 = 1;
+            led2 = 1;
+            led3 = 0.9;    
+            rht = 1;
+            debounceLeft.attach(&DBLeft, 0.100);
+        }
          //When not moving
-         if (x < -2000 && x > -3000 && y < 4000 && y > 3000 && midFl1 == 1){
-//            dwn = 0;
+        if (xUD < -2000 && xUD > -3000 && yUD < 4000 && yUD > 3000 && mFUp == 1){
             up = 0;
-            midFl1 = 0;
-            middle1.attach(&mid1,0.50);
+            mFUp = 0;
+            middleUp.attach(&midUp,0.50);
             led1 = 0.9;
             led2 = 1;
             led3 = 1;
-        }
+        } // end if
         
-         if (x < -1000 && x > -2800 && y < 3000 && y > 2000 && midFl2 == 1){
+         if (xUD < -1000 && xUD > -2800 && yUD < 3000 && yUD > 2000 && mFDown == 1){
             dwn = 0;
-//            up = 0;
-            midFl2 = 0;
-            middle2.attach(&mid2,0.50);
+            mFDown = 0;
+            middleDown.attach(&midDown,0.50);
             led1 = 0.9;
             led2 = 0.9;
             led3 = 1;
-        }
+        } // end if
+
+        if (xLR < -2000 && xLR > -3000 && yLR < 4000 && yLR > 3000 && mFLeft == 1){
+            lft = 0;
+            mFLeft = 0;
+            middleLeft.attach(&midLeft,0.50);
+            led1 = 0.9;
+            led2 = 1;
+            led3 = 1;
+        } // end if
+        
+         if (xLR < -1000 && xLR > -2800 && yLR < 3000 && yLR > 2000 && mFRight == 1){
+            rht = 0;
+            mFRight = 0;
+            middleRight.attach(&midRight,0.50);
+            led1 = 0.9;
+            led2 = 0.9;
+            led3 = 1;
+        } // end if
          
         wait_ms(50);
-    }
-}
+    } // end while
+} // end main
\ No newline at end of file