Diff: main.cpp

Revision:

0:5abac0d420fc

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Feb 23 13:36:41 2012 +0000
@@ -0,0 +1,303 @@
+#include "mbed.h"
+#include "EthernetNetIf.h"
+#include "TCPSocket.h"
+#include "ADXL345_I2C.h"
+
+DigitalOut led1(LED1, "led1");
+DigitalOut led2(LED2, "led2");
+DigitalOut led3(LED3, "led3");
+DigitalOut led4(LED4, "led4");
+AnalogIn ana_x(p19, "ana_x");
+AnalogIn ana_y(p20, "ana_y");
+DigitalIn init(p5);
+DigitalIn garbage(p6);
+DigitalIn closesock(p7);
+ADXL345_I2C accelerometer(p9, p10);
+
+
+EthernetNetIf eth(
+  IpAddr(192,168,0,16), //IP Address
+  IpAddr(255,255,255,0), //Network Mask
+  IpAddr(192,168,0,1), //Gateway
+  IpAddr(192,168,0,1) //DNS
+);
+
+
+// == stability vars
+Ticker routine;
+
+float base_x_0 = 50;
+float base_y_0 = 50;
+float base_x_1 = 50;
+float base_y_1 = 50;
+float base_x_2 = 50;
+float base_y_2 = 50;
+float base_x_3 = 50;
+float base_y_3 = 50;
+
+float motor_0 = 50;
+float motor_1 = 50;
+float motor_2 = 50;
+float motor_3 = 50;
+
+float a_motor_0 = motor_0;
+float a_motor_1 = motor_1;
+float a_motor_2 = motor_2;
+float a_motor_3 = motor_3;
+
+long x = 50;
+long y = 50;
+
+// == end
+
+int go = 0;
+int countertje = 0;
+
+#define TCP_LISTENING_PORT 1337
+
+TCPSocket ListeningSock; //Listener
+TCPSocket* pConnectedSock; // for ConnectedSock
+Host client; 
+TCPSocketErr err;
+char data[128];
+
+void sender(const char*);
+void interrupt();
+void onConnectedTCPSocketEvent( TCPSocketEvent );
+void onListeningTCPSocketEvent( TCPSocketEvent );
+
+int main() {
+
+    //Go into standby mode to configure the device.
+    accelerometer.setPowerControl(0x00);
+ 
+    //Full resolution, +/-16g, 4mg/LSB.
+    accelerometer.setDataFormatControl(0x0B);
+     
+    //3.2kHz data rate.
+    accelerometer.setDataRate(ADXL345_3200HZ);
+ 
+    //Measurement mode.
+    accelerometer.setPowerControl(0x08);
+
+    EthernetErr ethErr = eth.setup();
+      if(ethErr)
+      {
+        led3 = 1;
+        return -1;
+      }
+    // Set the callbacks for Listening
+    ListeningSock.setOnEvent(&onListeningTCPSocketEvent); 
+    // bind and listen on TCP
+    err=ListeningSock.bind(Host(IpAddr(), TCP_LISTENING_PORT)); // listen on own ipaddres, port 1337
+    if(err)
+    {
+    //Deal with that error...
+        led3 = 1;
+    }
+    err=ListeningSock.listen(); // Starts listening
+    if(err)
+    {
+         led3 = 1;
+    }
+    routine.attach(&interrupt, 1);
+    Timer timer;
+    timer.start();
+    while(true)
+    {
+        Net::poll(); //poll TCPstack
+        while(init) {
+            if(!init){
+                sender("220 Connected\n");
+                go=1;
+                break;
+                
+            }
+        }
+        while(closesock) {
+            if(!closesock){
+                led1=0;
+                go=0;
+                pConnectedSock->close();
+                break;
+            }
+        }
+        if(go){
+            if(countertje == 10){
+                countertje = 0;
+            }
+            char buffer [128];
+            sprintf (buffer, "%d\n",countertje);
+            sender(buffer);
+            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);
+            sender(buffer);
+            char receive[128];
+            while ( int len = pConnectedSock->recv(receive, 128) ) {
+              led2 != led2;
+              if(strcmp(receive,"Got it!")==0){
+                led2 = 0;
+                break;
+              }
+            }
+            /*
+            sprintf (buffer, "y: %d\n", y);
+            sender(buffer);
+            sprintf (buffer, "motor_0: %d\n", motor_0);
+            sender(buffer);
+            sprintf (buffer, "motor_1: %d\n", motor_1);
+            sender(buffer);
+            sprintf (buffer, "motor_2: %d\n", motor_2);
+            sender(buffer);
+            sprintf (buffer, "motor_3: %d\n", motor_3);
+            sender(buffer); */
+            countertje++;
+        }
+        if(timer.read() > 0.2) 
+        {
+            led4=!led4; //Status light toggle
+            timer.reset();
+        }
+  }
+}
+
+void sender( const char* data ){
+    int len = strlen(data);
+    pConnectedSock->send(data, len);
+}
+
+
+void interrupt() 
+{
+    x = accelerometer.getAx();
+    y = accelerometer.getAy();
+
+    a_motor_0 = a_motor_0-((((base_x_0-x)/2)+((base_x_0-y)/2))/1000);
+    if(a_motor_0>99){
+        motor_0 = 100;}
+    else if(a_motor_0 < 1){
+        motor_0 = 0;}
+    else{
+        motor_0 = a_motor_0;}
+    a_motor_1 = a_motor_1-((((base_x_1-x)/2)-((base_x_1-y)/2))/1000);
+    if(a_motor_1>99){
+        motor_1 = 100;}
+    else if(a_motor_1 < 1){
+        motor_1 = 0;}
+    else{
+        motor_1 = a_motor_1;}
+    a_motor_2 = a_motor_2+((((base_x_2-x)/2)-((base_x_2-y)/2))/1000);
+    if(a_motor_2>99){
+        motor_2 = 100;}
+    else if(a_motor_2 < 1){
+        motor_2 = 0;}
+    else{
+        motor_2 = a_motor_2;}
+    a_motor_3 = a_motor_3+((((base_x_3-x)/2)+((base_x_3-y)/2))/1000);
+    if(a_motor_3>99){
+        motor_3 = 100;}
+    else if(a_motor_3 < 1){
+        motor_3 = 0;}
+    else{
+        motor_3 = a_motor_3;}
+}
+
+void onConnectedTCPSocketEvent(TCPSocketEvent e)
+{
+   switch(e)
+    {
+    case TCPSOCKET_CONNECTED:
+        led1 = 1;
+        break;
+    case TCPSOCKET_WRITEABLE:
+      //Can now write some data...
+        led1 = 1;
+        break;
+    case TCPSOCKET_READABLE:
+      //Can now read dome data...
+        led1 = 1;
+       // Read in any available data into the buffer
+       char buff[128];
+       while ( int len = pConnectedSock->recv(buff, 128) ) {
+       // And send straight back out again
+           
+           //pConnectedSock->send(buff, len);
+           //buff[len]=0; // make terminater
+           led2 != led2;
+           strcpy(data,buff); 
+       }
+       break;
+    case TCPSOCKET_CONTIMEOUT:
+        printf("TCP Socket Timeout\r\n");
+        break;
+    case TCPSOCKET_CONRST:
+        printf("TCP Socket CONRST\r\n");
+        break;
+    case TCPSOCKET_CONABRT:
+        printf("TCP Socket CONABRT\r\n");
+        break;
+    case TCPSOCKET_ERROR:
+        printf("TCP Socket Error\r\n");
+        break;
+    case TCPSOCKET_DISCONNECTED:
+    //Close socket...
+        printf("TCP Socket Disconnected\r\n");        
+        pConnectedSock->close();
+        led1=0;
+        break;
+    default:
+        printf("DEFAULT\r\n"); 
+      }
+}
+
+
+void onListeningTCPSocketEvent(TCPSocketEvent e)
+{
+    switch(e)
+    {
+    case TCPSOCKET_ACCEPT:
+        printf("Listening: TCP Socket Accepted\r\n");
+        // Accepts connection from client and gets connected socket.   
+        err=ListeningSock.accept(&client, &pConnectedSock);
+        if (err) {
+            led3 = 1;
+            return; //Error in accept, discard connection
+        }
+        // Setup the new socket events
+        pConnectedSock->setOnEvent(&onConnectedTCPSocketEvent);
+        // We can find out from where the connection is coming by looking at the
+        // Host parameter of the accept() method
+        IpAddr clientIp = client.getIp();
+        printf("Listening: Incoming TCP connection from %d.%d.%d.%d\r\n", 
+           clientIp[0], clientIp[1], clientIp[2], clientIp[3]);
+       break;
+    // the following cases will not happen
+    case TCPSOCKET_CONNECTED:
+        printf("Listening: TCP Socket Connected\r\n");
+        break;
+    case TCPSOCKET_WRITEABLE:
+        printf("Listening: TCP Socket Writable\r\n");
+        break;
+    case TCPSOCKET_READABLE:
+        printf("Listening: TCP Socket Readable\r\n");
+        break;
+    case TCPSOCKET_CONTIMEOUT:
+        printf("Listening: TCP Socket Timeout\r\n");
+        break;
+    case TCPSOCKET_CONRST:
+        printf("Listening: TCP Socket CONRST\r\n");
+        break;
+    case TCPSOCKET_CONABRT:
+        printf("Listening: TCP Socket CONABRT\r\n");
+        break;
+    case TCPSOCKET_ERROR:
+        led3 = 1;
+        break;
+    case TCPSOCKET_DISCONNECTED:
+    //Close socket...
+        printf("Listening: TCP Socket Disconnected\r\n");        
+        ListeningSock.close();
+        break;
+    default:
+        printf("DEFAULT\r\n"); 
+     };
+}