main.cpp

00001 #include "mbed.h"
00002 #include "EthernetNetIf.h"
00003 #include "TCPSocket.h"
00004 #include "IMUTICKER.h"
00005 #include "SCP1000.h"
00006 #include "HMC6352.h"
00007 #include <iostream>
00008 #include <math.h>
00009 
00010 
00011 
00012 
00013 
00014 
00015 
00016 
00017 SCP1000 scp1(p11,p12,p13,p14);
00018 SCP1000 scp2(p11,p12,p13,p15);
00019 HMC6352 compass(p28, p27);
00020 
00021 
00022 //++++++++++++++++++++++++++Ethernet++++++++++++++++++++++++++++++
00023 int t0=0,t1=0,t2=0,t3=0;   
00024 Timer tmr;
00025 
00026 void ByteToChar(int bytes[29], char chars[29], unsigned int count) {
00027     for (unsigned int i = 0; i < count; i++)
00028         chars[i] = (char)bytes[i];
00029 }
00030 
00031 
00032 
00033 EthernetNetIf eth(
00034     IpAddr(10,0,0,100), //IP Address
00035     IpAddr(255,255,255,0), //Network Mask
00036     IpAddr(10,0,0,1), //Gateway
00037     IpAddr(10,0,0,1)  //DNS
00038 );
00039 
00040 
00041 // EthernetNetIf eth;
00042 
00043 DigitalOut connectionLED(LED2);
00044 DigitalOut statusLED(LED1);
00045 
00046 TCPSocket socket;
00047 
00048 void onTCPSocketEvent(TCPSocketEvent e) {
00049     switch (e) {
00050         case TCPSOCKET_CONNECTED: {
00051             // Tells me that I'm connected to the server
00052             pc.printf("Connected\r\n");
00053             connectionLED = 1;
00054             wait(0.5);
00055  
00056         int b[29] = { 0, 0, 0, 0, 2,0,0,0,0,0,0,0,85,49 };
00057         char m[29];
00058         ByteToChar(b,m,29);
00059         int sendError = socket.send(m,14);
00060         }
00061         break;
00062 
00063         case TCPSOCKET_READABLE: {
00064         
00065         
00066           // Get the data and send them back to pc
00067             char rxBuffer[29];
00068             int len = socket.recv(rxBuffer, sizeof(rxBuffer)-1);
00069             if ( len != 0 ) {
00070                 rxBuffer[len] = '\0';
00071 
00072                 pc.printf("Received:\r\n%s\r\n");
00073 
00074                 int sendError = socket.send(rxBuffer,(strlen(rxBuffer) + 1));
00075             }
00076         }
00077         break;
00078 
00079         case TCPSOCKET_WRITEABLE: {
00080 
00081 
00082             char m[29];
00083             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 };
00084 
00085             ByteToChar(b,m,27);
00086 
00087             int sendError = socket.send(m,sizeof(m));
00088  
00089 
00090         }
00091         break;
00092 
00093         case TCPSOCKET_CONTIMEOUT:
00094         case TCPSOCKET_CONRST:
00095         case TCPSOCKET_CONABRT:
00096         case TCPSOCKET_ERROR: {
00097             pc.printf("Connection Error: %i\r\n",e);
00098             socket.close();
00099         }
00100         break;
00101 
00102         case TCPSOCKET_DISCONNECTED: {
00103             pc.printf("Disconnected\r\n");
00104             socket.close();
00105             connectionLED = 0;
00106         }
00107         break;
00108     }
00109 }
00110 
00111 string binary( unsigned long n ) {
00112     char     result[ (sizeof( unsigned long ) * 8) + 1 ];
00113     unsigned index  = sizeof( unsigned long ) * 8;
00114     result[ index ] = '\0';
00115 
00116     do result[ --index ] = '0' + (n & 1);
00117     while (n >>= 1);
00118 
00119     return string( result + index );
00120 }
00121 
00122 
00123 int main() {
00124     pc.baud(9600);
00125 
00126     start();
00127 
00128     compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
00129 
00130 //+++++++++++++++++++++++++++++ETHERNET+++++++++++++++++++++++++
00131 
00132     // Ethernet setup, callback setup and server declaration
00133     EthernetErr ethErr = eth.setup();
00134     if (ethErr)
00135         error("Error %d in setup.\r\n", ethErr);
00136 
00137     socket.setOnEvent(&onTCPSocketEvent);
00138     Host server(IpAddr(10, 0, 0, 7), 1000);
00139     TCPSocketErr bindStatus = socket.connect(server);
00140     if (bindStatus != TCPSOCKET_OK)
00141     pc.printf("Bind error\r\n");
00142     
00143     tmr.start();
00144 
00145 
00146     while (true) 
00147     {
00148     
00149         Net::poll();
00150         
00151         
00152                    if (tmr.read() > 2) 
00153         {
00154         tmr.reset();
00155 
00156         statusLED = !statusLED;
00157         
00158         pc.printf("%f,%f,%f\n\r",toDegrees(imuFilter.getRoll()),toDegrees(imuFilter.getPitch()),toDegrees(imuFilter.getYaw()));
00159         pc.printf("The pressure is %d Pa,temperature is %f C\n\r", scp1.readPressure(), scp1.readTemperature()/100);
00160         pc.printf("The pressure is %d Pa,temperature is %f C\n\r", scp2.readPressure(), scp2.readTemperature()/100);
00161         pc.printf("Heading is: %f", compass.sample() / 10.0);
00162 
00163         float rol=toDegrees(imuFilter.getRoll());
00164         int i=0;
00165         int r[16];
00166         double bol=3;
00167         while (rol>2) {
00168             bol=int(floor(rol/2));
00169             r[i]=floor(rol-(bol*2));
00170             rol=((rol-r[i])/2);
00171             i=i+1;
00172         }
00173         bol=int(floor(rol/2));
00174         r[i]=floor(rol-(bol*2));
00175         r[i+1]=1;
00176         for (int ii=i+2; ii<16; ii++) {
00177             r[ii]=0;
00178         }
00179         int kov[16];
00180         for (int ii=0; ii<8; ii++) {
00181             kov[ii]=r[ii];
00182             r[ii]=r[15-ii];
00183             r[15-ii]=kov[ii];
00184         }
00185 
00186         rol=toDegrees(imuFilter.getPitch());
00187         i=0;
00188         int p[16];
00189         bol=3;
00190         while (rol>2) {
00191             bol=(floor(rol/2));
00192             p[i]=floor(rol-(bol*2));
00193             rol=((rol-p[i])/2);
00194             i=i+1;
00195         }
00196         bol=int(floor(rol/2));
00197         p[i]=floor(rol-(bol*2));
00198         p[i+1]=1;
00199         for (int ii=i+2; ii<16; ii++) {
00200             p[ii]=0;
00201         }
00202         for (int ii=0; ii<8; ii++) {
00203             kov[ii]=p[ii];
00204             p[ii]=p[15-ii];
00205             p[15-ii]=kov[ii];
00206         }
00207 
00208         rol=toDegrees(imuFilter.getYaw());
00209         i=0;
00210         int y[16];
00211         bol=3;
00212         while (rol>2) {
00213             bol=(floor(rol/2));
00214             y[i]=floor(rol-(bol*2));
00215             rol=((rol-y[i])/2);
00216             i=i+1;
00217         }
00218         bol=(floor(rol/2));
00219         y[i]=floor(rol-(bol*2));
00220         y[i+1]=1;
00221         for (int ii=i+2; ii<16; ii++) {
00222             y[ii]=0;
00223         }
00224         for (int ii=0; ii<8; ii++) {
00225             kov[ii]=y[ii];
00226             y[ii]=y[15-ii];
00227             y[15-ii]=kov[ii];
00228         }
00229 
00230         char pp[32];
00231         for (int ii=0; ii<9; ii++) {
00232             pp[ii]=p[ii+8];
00233         }
00234         pp[8]=p[7];
00235 
00236         for (int ii=0; ii<9; ii++) {
00237             pp[ii+9]=r[ii+8];
00238         }
00239         pp[17]=r[7];
00240 
00241         for (int ii=0; ii<9; ii++) {
00242             pp[ii+18]=y[ii+8];
00243         }
00244         pp[26]=y[7];
00245 
00246         for (int ii=0; ii<6; ii++) {
00247             pp[ii+27]=0;
00248         }
00249 
00250         char by[3][7];
00251 
00252         for (int ii=0; ii<4; ii++) {
00253             for (int jj=0; jj<8; jj++) {
00254                 by[ii][jj]=pp[(ii*8)+jj];
00255             }
00256         }
00257 
00258         char by0[8],by1[8],by2[8],by3[8];
00259         for (int ii=0; ii<8; ii++) {
00260             by0[ii]=by[0][ii];
00261         }
00262         for (int ii=0; ii<8; ii++) {
00263             by1[ii]=by[1][ii];
00264         }
00265         for (int ii=0; ii<8; ii++) {
00266             by2[ii]=by[2][ii];
00267         }
00268         for (int ii=0; ii<8; ii++) {
00269             by3[ii]=by[3][ii];
00270         }
00271 
00272 
00273 
00274         for (int ii=0; ii<8; ii++) {
00275             i=by0[ii];
00276             t0=t0+(i*pow(2.0,ii));
00277         }
00278         for (int ii=0; ii<8; ii++) {
00279             i=by1[ii];
00280             t1=t1+(i*pow(2.0,ii));
00281         }
00282         for (int ii=0; ii<8; ii++) {
00283             i=by2[ii];
00284             t2=t2+(i*pow(2.0,ii));
00285         }
00286         for (int ii=0; ii<8; ii++) {
00287             i=by3[ii];
00288 
00289             t3=t3+(i*pow(2.0,ii));
00290         }
00291 //-----------------------------------------------------
00292 }
00293 
00294 //+++++++++++++++++++++++++++++IMU+++++++++++++++++++++++++++++++++++
00295 
00296     }
00297 }