Diff: main.cpp

Revision:

1:a88d1309f810

Parent:

0:5de940cf9783

Child:

2:96f81996a332

--- a/main.cpp	Thu Mar 01 15:25:57 2012 +0000
+++ b/main.cpp	Fri Mar 02 22:31:23 2012 +0000
@@ -27,46 +27,63 @@
 
 */
 
+
+//##############  INCLUDE FILES  ##############//
 #include "mbed.h"
 #include "EthernetNetIf.h"
 #include "TCPSocket.h"
 #include "ADXL345_I2C.h"
+//#############################################//
 
+//##############     DEFINES     ##############//
 #define TCP_LISTENING_PORT 1337
-int go = 0;
+#define PI 3.14159265
+//#############################################//
 
+//##############    VARIABLES    ##############//
+double  x_cor       = 0, 
+        y_cor       = 0, 
+        cont[2]     = {0,0};
+int     print       = 0,
+        go          = 0,
+        calibrated  = 0;
+//#############################################//
+
+//##############PIN CONFIGURATION##############//
 DigitalOut led3(LED3, "led3");
 DigitalOut led4(LED4, "led4");
 DigitalIn init(p5);
 DigitalIn calibrate(p20);
 //I2C i2c(p9, p10); // sda, scl
 ADXL345_I2C acc(p9, p10);
+//#############################################//
 
-
+//##############    PROTOTYPES   ##############//
 void tcp_send(const char*);
 void tcp_sendIfConnected( const char* data );
 void onConnectedTCPSocketEvent(TCPSocketEvent e);
 void onListeningTCPSocketEvent(TCPSocketEvent e);
+double getAngle(char axis, int precision);
+//#############################################//
 
-
+//############## ETHERNET CONFIG ##############//
 // EthernetNetIf eth;
 EthernetNetIf eth(
     IpAddr(192,168,0,16), //IP Address
     IpAddr(255,255,255,0), //Network Mask
-    IpAddr(192,168,0,2), //Gateway
-    IpAddr(192,168,0,2)  //DNS
+    IpAddr(192,168,0,1), //Gateway
+    IpAddr(192,168,0,1)  //DNS
 );
 
 TCPSocket ListeningSock;
 TCPSocket* pConnectedSock; // for ConnectedSock
 Host client;
 TCPSocketErr err;
+//#############################################//
+
 Ticker routine;
 
-uint16_t x_cor = 0; 
-uint16_t y_cor = 0; 
-uint16_t z_cor = 0; 
-
+//##############    INTERRUPT    ##############//
 void interrupt() 
 {
 //    int count = 0;
@@ -81,54 +98,58 @@
 //             count++;
 //        }
 //    } 
+    if(go){
+        if(calibrated){
+            if(print){
+                char buffer [128];
+                sprintf (buffer, "x:%f - y:%f\n",cont[0],cont[1]);
+                tcp_send(buffer);  
+                print = 0;
+            }
+        }
+    }
+}
+//#############################################//
 
-    int readings[3] = {0, 0, 0};
-    if(go)acc.getOutput(readings);
-    char buffer [128];
-    if(go)sprintf (buffer, "x:%i - y:%i - z:%i\n",(int16_t)readings[0],(int16_t)readings[1],(int16_t)readings[2]);
-    if(go)tcp_send(buffer);  
-}
-
+//##############       MAIN      ##############//
 int main() {
-
+    //##############ETHERNET&TCP INIT##############//
     EthernetErr ethErr = eth.setup();
     if(ethErr)
     {
         return -1;
-    }
-    
-    IpAddr ip = eth.getIp();
-    
+    }    
+    IpAddr ip = eth.getIp();    
     // Set the callbacks for Listening
-    ListeningSock.setOnEvent(&onListeningTCPSocketEvent); 
-    
+    ListeningSock.setOnEvent(&onListeningTCPSocketEvent);    
     // bind and listen on TCP
     err=ListeningSock.bind(Host(IpAddr(), TCP_LISTENING_PORT));
     if(err)
     {
         //Deal with that error...
-    }
-    
+    }    
     err=ListeningSock.listen(); // Starts listening
     if(err)
     {
         //listening error
     }
+    //#############################################//
     
+    //#############ACCELEROMETER INIT##############//
     //Go into standby mode to configure the device.
     acc.setPowerControl(0x00);
- 
     //Full resolution, +/-16g, 4mg/LSB.
-    acc.setDataFormatControl(0x0B);
-     
+    acc.setDataFormatControl(0x00);    
     //3.2kHz data rate.
     acc.setDataRate(ADXL345_3200HZ);
- 
     //Measurement mode.
     acc.setPowerControl(0x08);
     wait(0.1);
+    //#############################################//
+    
     Timer tmr;
     tmr.start();
+    
     while(true)
     {
         Net::poll();
@@ -148,15 +169,35 @@
         }
         if(calibrate){
             while(calibrate){};
-            int readings[3] = {0, 0, 0};
-            if(go)acc.getOutput(readings);
-            x_cor = readings[0];
-            y_cor = readings[1];
-            z_cor = readings[2];
-            routine.attach_us(&interrupt, 10);
+            x_cor = getAngle('x',100);
+            y_cor = getAngle('y',100);
+           
+            char buffer [128];
+            sprintf (buffer, "Calibration values:\n");
+            tcp_send(buffer);
+            sprintf (buffer, "x:%f - y:%f\n",x_cor, y_cor);
+            tcp_send(buffer); 
+            sprintf (buffer, "------------------------\n");
+            tcp_send(buffer);
+             
+            calibrated = 1;
+            
+            wait(0.1);
+            
+            routine.attach_us(&interrupt,10);
+        }
+        if(calibrated){
+
+            cont[0] = getAngle('x',10) - x_cor;
+            cont[1] = getAngle('y',10) - y_cor;
+
+            print = 1;
         }
     }  
 }
+//#############################################//
+
+//##############   SUBROUTINES   ##############//
 
 void tcp_send( const char* data ){
     int len = strlen(data);
@@ -256,3 +297,33 @@
         printf("DEFAULT\r\n"); 
      };
 }
+
+double getAngle(char axis, int precision)
+{
+    int     measurement[3] = {0,0,0};
+    double  x_temp  = 0,
+            y_temp  = 0,
+            z_temp  = 0,
+            x_angle = 0,
+            y_angle = 0;
+            
+    int     i;
+    
+    for(i=0;i<precision;i++)
+    {
+        acc.getOutput(measurement);
+        x_temp = (int16_t)measurement[0];
+        y_temp = (int16_t)measurement[1];
+        z_temp = (int16_t)measurement[2];
+        x_angle += x_temp/z_temp;
+        y_angle += y_temp/z_temp; 
+    }
+    x_angle = x_angle/precision;
+    y_angle = y_angle/precision;
+//    x_angle = atan(x_angle) * 180 / PI;
+//    y_angle = atan(y_angle) * 180 / PI;
+    if(axis == 'x')return atan(x_angle) * 180 / PI;
+    if(axis == 'y')return atan(y_angle) * 180 / PI;
+}
+
+