main.cpp

00001 #include "mbed.h"
00002 #include "EthernetNetIf.h"
00003 #include "TCPSocket.h"
00004 #include "ADXL345_I2C.h"
00005 
00006 DigitalOut led1(LED1, "led1");
00007 DigitalOut led2(LED2, "led2");
00008 DigitalOut led3(LED3, "led3");
00009 DigitalOut led4(LED4, "led4");
00010 AnalogIn ana_x(p19, "ana_x");
00011 AnalogIn ana_y(p20, "ana_y");
00012 DigitalIn init(p5);
00013 DigitalIn garbage(p6);
00014 DigitalIn closesock(p7);
00015 ADXL345_I2C accelerometer(p9, p10);
00016 
00017 
00018 EthernetNetIf eth(
00019   IpAddr(192,168,0,16), //IP Address
00020   IpAddr(255,255,255,0), //Network Mask
00021   IpAddr(192,168,0,1), //Gateway
00022   IpAddr(192,168,0,1) //DNS
00023 );
00024 
00025 
00026 // == stability vars
00027 Ticker routine;
00028 
00029 float base_x_0 = 50;
00030 float base_y_0 = 50;
00031 float base_x_1 = 50;
00032 float base_y_1 = 50;
00033 float base_x_2 = 50;
00034 float base_y_2 = 50;
00035 float base_x_3 = 50;
00036 float base_y_3 = 50;
00037 
00038 float motor_0 = 50;
00039 float motor_1 = 50;
00040 float motor_2 = 50;
00041 float motor_3 = 50;
00042 
00043 float a_motor_0 = motor_0;
00044 float a_motor_1 = motor_1;
00045 float a_motor_2 = motor_2;
00046 float a_motor_3 = motor_3;
00047 
00048 long x = 50;
00049 long y = 50;
00050 
00051 // == end
00052 
00053 int go = 0;
00054 int countertje = 0;
00055 
00056 #define TCP_LISTENING_PORT 1337
00057 
00058 TCPSocket ListeningSock; //Listener
00059 TCPSocket* pConnectedSock; // for ConnectedSock
00060 Host client; 
00061 TCPSocketErr err;
00062 char data[128];
00063 
00064 void sender(const char*);
00065 void interrupt();
00066 void onConnectedTCPSocketEvent( TCPSocketEvent );
00067 void onListeningTCPSocketEvent( TCPSocketEvent );
00068 
00069 int main() {
00070 
00071     //Go into standby mode to configure the device.
00072     accelerometer.setPowerControl(0x00);
00073  
00074     //Full resolution, +/-16g, 4mg/LSB.
00075     accelerometer.setDataFormatControl(0x0B);
00076      
00077     //3.2kHz data rate.
00078     accelerometer.setDataRate(ADXL345_3200HZ);
00079  
00080     //Measurement mode.
00081     accelerometer.setPowerControl(0x08);
00082 
00083     EthernetErr ethErr = eth.setup();
00084       if(ethErr)
00085       {
00086         led3 = 1;
00087         return -1;
00088       }
00089     // Set the callbacks for Listening
00090     ListeningSock.setOnEvent(&onListeningTCPSocketEvent); 
00091     // bind and listen on TCP
00092     err=ListeningSock.bind(Host(IpAddr(), TCP_LISTENING_PORT)); // listen on own ipaddres, port 1337
00093     if(err)
00094     {
00095     //Deal with that error...
00096         led3 = 1;
00097     }
00098     err=ListeningSock.listen(); // Starts listening
00099     if(err)
00100     {
00101          led3 = 1;
00102     }
00103     routine.attach(&interrupt, 1);
00104     Timer timer;
00105     timer.start();
00106     while(true)
00107     {
00108         Net::poll(); //poll TCPstack
00109         while(init) {
00110             if(!init){
00111                 sender("220 Connected\n");
00112                 go=1;
00113                 break;
00114                 
00115             }
00116         }
00117         while(closesock) {
00118             if(!closesock){
00119                 led1=0;
00120                 go=0;
00121                 pConnectedSock->close();
00122                 break;
00123             }
00124         }
00125         if(go){
00126             if(countertje == 10){
00127                 countertje = 0;
00128             }
00129             char buffer [128];
00130             sprintf (buffer, "%d\n",countertje);
00131             sender(buffer);
00132             sprintf (buffer, "x: %d\ny: %d\nmotor_0: %f\nmotor_1: %f\nmotor_2: %f\nmotor_3: %f\n", x,y,motor_0,motor_1,motor_2,motor_3);
00133             sender(buffer);
00134             char receive[128];
00135             while ( int len = pConnectedSock->recv(receive, 128) ) {
00136               led2 != led2;
00137               if(strcmp(receive,"Got it!")==0){
00138                 led2 = 0;
00139                 break;
00140               }
00141             }
00142             /*
00143             sprintf (buffer, "y: %d\n", y);
00144             sender(buffer);
00145             sprintf (buffer, "motor_0: %d\n", motor_0);
00146             sender(buffer);
00147             sprintf (buffer, "motor_1: %d\n", motor_1);
00148             sender(buffer);
00149             sprintf (buffer, "motor_2: %d\n", motor_2);
00150             sender(buffer);
00151             sprintf (buffer, "motor_3: %d\n", motor_3);
00152             sender(buffer); */
00153             countertje++;
00154         }
00155         if(timer.read() > 0.2) 
00156         {
00157             led4=!led4; //Status light toggle
00158             timer.reset();
00159         }
00160   }
00161 }
00162 
00163 void sender( const char* data ){
00164     int len = strlen(data);
00165     pConnectedSock->send(data, len);
00166 }
00167 
00168 
00169 void interrupt() 
00170 {
00171     x = accelerometer.getAx();
00172     y = accelerometer.getAy();
00173 
00174     a_motor_0 = a_motor_0-((((base_x_0-x)/2)+((base_x_0-y)/2))/1000);
00175     if(a_motor_0>99){
00176         motor_0 = 100;}
00177     else if(a_motor_0 < 1){
00178         motor_0 = 0;}
00179     else{
00180         motor_0 = a_motor_0;}
00181     a_motor_1 = a_motor_1-((((base_x_1-x)/2)-((base_x_1-y)/2))/1000);
00182     if(a_motor_1>99){
00183         motor_1 = 100;}
00184     else if(a_motor_1 < 1){
00185         motor_1 = 0;}
00186     else{
00187         motor_1 = a_motor_1;}
00188     a_motor_2 = a_motor_2+((((base_x_2-x)/2)-((base_x_2-y)/2))/1000);
00189     if(a_motor_2>99){
00190         motor_2 = 100;}
00191     else if(a_motor_2 < 1){
00192         motor_2 = 0;}
00193     else{
00194         motor_2 = a_motor_2;}
00195     a_motor_3 = a_motor_3+((((base_x_3-x)/2)+((base_x_3-y)/2))/1000);
00196     if(a_motor_3>99){
00197         motor_3 = 100;}
00198     else if(a_motor_3 < 1){
00199         motor_3 = 0;}
00200     else{
00201         motor_3 = a_motor_3;}
00202 }
00203 
00204 void onConnectedTCPSocketEvent(TCPSocketEvent e)
00205 {
00206    switch(e)
00207     {
00208     case TCPSOCKET_CONNECTED:
00209         led1 = 1;
00210         break;
00211     case TCPSOCKET_WRITEABLE:
00212       //Can now write some data...
00213         led1 = 1;
00214         break;
00215     case TCPSOCKET_READABLE:
00216       //Can now read dome data...
00217         led1 = 1;
00218        // Read in any available data into the buffer
00219        char buff[128];
00220        while ( int len = pConnectedSock->recv(buff, 128) ) {
00221        // And send straight back out again
00222            
00223            //pConnectedSock->send(buff, len);
00224            //buff[len]=0; // make terminater
00225            led2 != led2;
00226            strcpy(data,buff); 
00227        }
00228        break;
00229     case TCPSOCKET_CONTIMEOUT:
00230         printf("TCP Socket Timeout\r\n");
00231         break;
00232     case TCPSOCKET_CONRST:
00233         printf("TCP Socket CONRST\r\n");
00234         break;
00235     case TCPSOCKET_CONABRT:
00236         printf("TCP Socket CONABRT\r\n");
00237         break;
00238     case TCPSOCKET_ERROR:
00239         printf("TCP Socket Error\r\n");
00240         break;
00241     case TCPSOCKET_DISCONNECTED:
00242     //Close socket...
00243         printf("TCP Socket Disconnected\r\n");        
00244         pConnectedSock->close();
00245         led1=0;
00246         break;
00247     default:
00248         printf("DEFAULT\r\n"); 
00249       }
00250 }
00251 
00252 
00253 void onListeningTCPSocketEvent(TCPSocketEvent e)
00254 {
00255     switch(e)
00256     {
00257     case TCPSOCKET_ACCEPT:
00258         printf("Listening: TCP Socket Accepted\r\n");
00259         // Accepts connection from client and gets connected socket.   
00260         err=ListeningSock.accept(&client, &pConnectedSock);
00261         if (err) {
00262             led3 = 1;
00263             return; //Error in accept, discard connection
00264         }
00265         // Setup the new socket events
00266         pConnectedSock->setOnEvent(&onConnectedTCPSocketEvent);
00267         // We can find out from where the connection is coming by looking at the
00268         // Host parameter of the accept() method
00269         IpAddr clientIp = client.getIp();
00270         printf("Listening: Incoming TCP connection from %d.%d.%d.%d\r\n", 
00271            clientIp[0], clientIp[1], clientIp[2], clientIp[3]);
00272        break;
00273     // the following cases will not happen
00274     case TCPSOCKET_CONNECTED:
00275         printf("Listening: TCP Socket Connected\r\n");
00276         break;
00277     case TCPSOCKET_WRITEABLE:
00278         printf("Listening: TCP Socket Writable\r\n");
00279         break;
00280     case TCPSOCKET_READABLE:
00281         printf("Listening: TCP Socket Readable\r\n");
00282         break;
00283     case TCPSOCKET_CONTIMEOUT:
00284         printf("Listening: TCP Socket Timeout\r\n");
00285         break;
00286     case TCPSOCKET_CONRST:
00287         printf("Listening: TCP Socket CONRST\r\n");
00288         break;
00289     case TCPSOCKET_CONABRT:
00290         printf("Listening: TCP Socket CONABRT\r\n");
00291         break;
00292     case TCPSOCKET_ERROR:
00293         led3 = 1;
00294         break;
00295     case TCPSOCKET_DISCONNECTED:
00296     //Close socket...
00297         printf("Listening: TCP Socket Disconnected\r\n");        
00298         ListeningSock.close();
00299         break;
00300     default:
00301         printf("DEFAULT\r\n"); 
00302      };
00303 }