Diff: main.cpp

Revision:

0:711905e937b9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Jul 24 19:49:51 2011 +0000
@@ -0,0 +1,297 @@
+#include "mbed.h"
+#include "EthernetNetIf.h"
+#include "TCPSocket.h"
+#include "IMUTICKER.h"
+#include "SCP1000.h"
+#include "HMC6352.h"
+#include <iostream>
+#include <math.h>
+
+
+
+
+
+
+
+
+SCP1000 scp1(p11,p12,p13,p14);
+SCP1000 scp2(p11,p12,p13,p15);
+HMC6352 compass(p28, p27);
+
+
+//++++++++++++++++++++++++++Ethernet++++++++++++++++++++++++++++++
+int t0=0,t1=0,t2=0,t3=0;   
+Timer tmr;
+
+void ByteToChar(int bytes[29], char chars[29], unsigned int count) {
+    for (unsigned int i = 0; i < count; i++)
+        chars[i] = (char)bytes[i];
+}
+
+
+
+EthernetNetIf eth(
+    IpAddr(10,0,0,100), //IP Address
+    IpAddr(255,255,255,0), //Network Mask
+    IpAddr(10,0,0,1), //Gateway
+    IpAddr(10,0,0,1)  //DNS
+);
+
+
+// EthernetNetIf eth;
+
+DigitalOut connectionLED(LED2);
+DigitalOut statusLED(LED1);
+
+TCPSocket socket;
+
+void onTCPSocketEvent(TCPSocketEvent e) {
+    switch (e) {
+        case TCPSOCKET_CONNECTED: {
+            // Tells me that I'm connected to the server
+            pc.printf("Connected\r\n");
+            connectionLED = 1;
+            wait(0.5);
+ 
+        int b[29] = { 0, 0, 0, 0, 2,0,0,0,0,0,0,0,85,49 };
+        char m[29];
+        ByteToChar(b,m,29);
+        int sendError = socket.send(m,14);
+        }
+        break;
+
+        case TCPSOCKET_READABLE: {
+        
+        
+          // Get the data and send them back to pc
+            char rxBuffer[29];
+            int len = socket.recv(rxBuffer, sizeof(rxBuffer)-1);
+            if ( len != 0 ) {
+                rxBuffer[len] = '\0';
+
+                pc.printf("Received:\r\n%s\r\n");
+
+                int sendError = socket.send(rxBuffer,(strlen(rxBuffer) + 1));
+            }
+        }
+        break;
+
+        case TCPSOCKET_WRITEABLE: {
+
+
+            char m[29];
+            int b[29] = { 2, 0, 0, 0, 2,0,0,0,15,0,0,0,85,49,64,t0,t1,t2,t3,255,255,255,255,255,255,255,255,255,255 };
+
+            ByteToChar(b,m,27);
+
+            int sendError = socket.send(m,sizeof(m));
+ 
+
+        }
+        break;
+
+        case TCPSOCKET_CONTIMEOUT:
+        case TCPSOCKET_CONRST:
+        case TCPSOCKET_CONABRT:
+        case TCPSOCKET_ERROR: {
+            pc.printf("Connection Error: %i\r\n",e);
+            socket.close();
+        }
+        break;
+
+        case TCPSOCKET_DISCONNECTED: {
+            pc.printf("Disconnected\r\n");
+            socket.close();
+            connectionLED = 0;
+        }
+        break;
+    }
+}
+
+string binary( unsigned long n ) {
+    char     result[ (sizeof( unsigned long ) * 8) + 1 ];
+    unsigned index  = sizeof( unsigned long ) * 8;
+    result[ index ] = '\0';
+
+    do result[ --index ] = '0' + (n & 1);
+    while (n >>= 1);
+
+    return string( result + index );
+}
+
+
+int main() {
+    pc.baud(9600);
+
+    start();
+
+    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+
+//+++++++++++++++++++++++++++++ETHERNET+++++++++++++++++++++++++
+
+    // Ethernet setup, callback setup and server declaration
+    EthernetErr ethErr = eth.setup();
+    if (ethErr)
+        error("Error %d in setup.\r\n", ethErr);
+
+    socket.setOnEvent(&onTCPSocketEvent);
+    Host server(IpAddr(10, 0, 0, 7), 1000);
+    TCPSocketErr bindStatus = socket.connect(server);
+    if (bindStatus != TCPSOCKET_OK)
+    pc.printf("Bind error\r\n");
+    
+    tmr.start();
+
+
+    while (true) 
+    {
+    
+        Net::poll();
+        
+        
+                   if (tmr.read() > 2) 
+        {
+        tmr.reset();
+
+        statusLED = !statusLED;
+        
+        pc.printf("%f,%f,%f\n\r",toDegrees(imuFilter.getRoll()),toDegrees(imuFilter.getPitch()),toDegrees(imuFilter.getYaw()));
+        pc.printf("The pressure is %d Pa,temperature is %f C\n\r", scp1.readPressure(), scp1.readTemperature()/100);
+        pc.printf("The pressure is %d Pa,temperature is %f C\n\r", scp2.readPressure(), scp2.readTemperature()/100);
+        pc.printf("Heading is: %f", compass.sample() / 10.0);
+
+        float rol=toDegrees(imuFilter.getRoll());
+        int i=0;
+        int r[16];
+        double bol=3;
+        while (rol>2) {
+            bol=int(floor(rol/2));
+            r[i]=floor(rol-(bol*2));
+            rol=((rol-r[i])/2);
+            i=i+1;
+        }
+        bol=int(floor(rol/2));
+        r[i]=floor(rol-(bol*2));
+        r[i+1]=1;
+        for (int ii=i+2; ii<16; ii++) {
+            r[ii]=0;
+        }
+        int kov[16];
+        for (int ii=0; ii<8; ii++) {
+            kov[ii]=r[ii];
+            r[ii]=r[15-ii];
+            r[15-ii]=kov[ii];
+        }
+
+        rol=toDegrees(imuFilter.getPitch());
+        i=0;
+        int p[16];
+        bol=3;
+        while (rol>2) {
+            bol=(floor(rol/2));
+            p[i]=floor(rol-(bol*2));
+            rol=((rol-p[i])/2);
+            i=i+1;
+        }
+        bol=int(floor(rol/2));
+        p[i]=floor(rol-(bol*2));
+        p[i+1]=1;
+        for (int ii=i+2; ii<16; ii++) {
+            p[ii]=0;
+        }
+        for (int ii=0; ii<8; ii++) {
+            kov[ii]=p[ii];
+            p[ii]=p[15-ii];
+            p[15-ii]=kov[ii];
+        }
+
+        rol=toDegrees(imuFilter.getYaw());
+        i=0;
+        int y[16];
+        bol=3;
+        while (rol>2) {
+            bol=(floor(rol/2));
+            y[i]=floor(rol-(bol*2));
+            rol=((rol-y[i])/2);
+            i=i+1;
+        }
+        bol=(floor(rol/2));
+        y[i]=floor(rol-(bol*2));
+        y[i+1]=1;
+        for (int ii=i+2; ii<16; ii++) {
+            y[ii]=0;
+        }
+        for (int ii=0; ii<8; ii++) {
+            kov[ii]=y[ii];
+            y[ii]=y[15-ii];
+            y[15-ii]=kov[ii];
+        }
+
+        char pp[32];
+        for (int ii=0; ii<9; ii++) {
+            pp[ii]=p[ii+8];
+        }
+        pp[8]=p[7];
+
+        for (int ii=0; ii<9; ii++) {
+            pp[ii+9]=r[ii+8];
+        }
+        pp[17]=r[7];
+
+        for (int ii=0; ii<9; ii++) {
+            pp[ii+18]=y[ii+8];
+        }
+        pp[26]=y[7];
+
+        for (int ii=0; ii<6; ii++) {
+            pp[ii+27]=0;
+        }
+
+        char by[3][7];
+
+        for (int ii=0; ii<4; ii++) {
+            for (int jj=0; jj<8; jj++) {
+                by[ii][jj]=pp[(ii*8)+jj];
+            }
+        }
+
+        char by0[8],by1[8],by2[8],by3[8];
+        for (int ii=0; ii<8; ii++) {
+            by0[ii]=by[0][ii];
+        }
+        for (int ii=0; ii<8; ii++) {
+            by1[ii]=by[1][ii];
+        }
+        for (int ii=0; ii<8; ii++) {
+            by2[ii]=by[2][ii];
+        }
+        for (int ii=0; ii<8; ii++) {
+            by3[ii]=by[3][ii];
+        }
+
+
+
+        for (int ii=0; ii<8; ii++) {
+            i=by0[ii];
+            t0=t0+(i*pow(2.0,ii));
+        }
+        for (int ii=0; ii<8; ii++) {
+            i=by1[ii];
+            t1=t1+(i*pow(2.0,ii));
+        }
+        for (int ii=0; ii<8; ii++) {
+            i=by2[ii];
+            t2=t2+(i*pow(2.0,ii));
+        }
+        for (int ii=0; ii<8; ii++) {
+            i=by3[ii];
+
+            t3=t3+(i*pow(2.0,ii));
+        }
+//-----------------------------------------------------
+}
+
+//+++++++++++++++++++++++++++++IMU+++++++++++++++++++++++++++++++++++
+
+    }
+}