FAN ARM UDP Server controlled with WiFi

Dependencies:   WizFi250Interface mbed

Fork of WizFi250_AP_HelloWorld by WIZnet

Files at this revision

API Documentation at this revision

Comitter:
joon874
Date:
Wed Sep 23 00:50:09 2015 +0000
Parent:
0:87d959d6a3f3
Commit message:
FAN ARM UDP Server controlled with WiFi

Changed in this revision

WizFi250Interface.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed-src.lib Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r 87d959d6a3f3 -r 8d6d624124fd WizFi250Interface.lib
--- a/WizFi250Interface.lib	Tue Jul 14 06:55:18 2015 +0000
+++ b/WizFi250Interface.lib	Wed Sep 23 00:50:09 2015 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/kaizen/code/WizFi250Interface/#1f5121c5efcd
+http://mbed.org/users/kaizen/code/WizFi250Interface/#3a2282277ad0
diff -r 87d959d6a3f3 -r 8d6d624124fd main.cpp
--- a/main.cpp	Tue Jul 14 06:55:18 2015 +0000
+++ b/main.cpp	Wed Sep 23 00:50:09 2015 +0000
@@ -21,6 +21,12 @@
 #include "mbed.h"
 #include "WizFi250Interface.h"
 
+/* AX-12 */
+#define AX12_REG_GOAL_POSITION 0x1E
+#define AX12_REG_MOVING 0x2E
+#define AX_Init 330
+
+#define SERVER_PORT    5000
 
 #define SECURE WizFi250::SEC_WPA2_MIXED
 #define SSID "WizFi250_AP_Test"
@@ -31,12 +37,23 @@
     Serial pc(USBTX, USBRX);
 #endif
 
+PwmOut Fan(D3);
 
-/**
- *  \brief Hello World
- *  \param none
- *  \return int
- */
+DigitalOut red(LED1);
+DigitalOut green(LED2);
+
+
+void UDPServer();
+
+/* AX-12 */
+int HeadUD = 200;
+int HeadRL = AX_Init;
+ 
+void SetGoal(int ID, int degrees, int flags);
+int write(int ID, int start, int bytes, char* data, int flag);
+int read(int ID, int start, int bytes, char* data);
+int isMoving(int ID);
+
 int main()
 {
     pc.baud(115200);
@@ -47,5 +64,401 @@
     if ( wizfi250.connect(SECURE, SSID, PASS, WizFi250::WM_AP))      return -1;
     printf("IP Address is %s\r\n", wizfi250.getIPAddress());
     
+    wait(1.0);
+    UDPServer();
+    
     wizfi250.disconnect();
-}
\ No newline at end of file
+}
+
+
+void UDPServer(){
+    UDPSocket server;
+    
+    printf("Socket opened\r\n");
+    
+    server.set_blocking(false);
+    server.bind(SERVER_PORT);
+    
+    printf("port open\r\n");
+    
+    Endpoint client;
+    
+    printf("endpoint created");
+    
+    char buffer[10];
+    
+    int move1 = AX_Init;
+    int move2 = AX_Init;
+        
+        while(true)
+        {
+            int n = server.receiveFrom(client, buffer, sizeof(buffer));
+            
+            if(n > 0)
+            {
+                //printf("Received packet from: %s\n", client.get_address());
+                
+                buffer[n] = '\0';
+
+                if(strstr(buffer, "down"))
+                {   
+                    printf("down\r\n");
+                    if(move1 >= 800){
+                        move1 = move1;
+                        SetGoal(8, move1, 1);
+                    }else{
+                        move1 += 20;
+                        SetGoal(8, move1, 1);
+                    }
+                }
+                else if(strstr(buffer, "up"))
+                {
+                    printf("up\r\n");
+                    if(move1 == 200){
+                        move1 = move1;
+                        SetGoal(8, move1, 1);
+                    }else{
+                        move1 -= 20;
+                        SetGoal(8, move1, 1);
+                    }
+                }
+                else if(strstr(buffer, "left"))
+                {
+                    printf("left\r\n");   
+                    if(move2 >= 800){
+                        move2 = move2;
+                        SetGoal(16, move2, 1);
+                    }else{
+                        move2 += 20;
+                        SetGoal(16, move2, 1);
+                    }
+                }
+                else if(strstr(buffer, "right"))
+                {
+                    printf("right\r\n");
+                    if(move2 == 200){
+                        move2 = move2;
+                        SetGoal(16, move2, 1);
+                    }else{
+                        move2 -= 20;
+                        SetGoal(16, move2, 1);
+                    }
+                }
+                else if(strstr(buffer, "init"))
+                {
+                    printf("init\r\n");
+                    move1 = move1;
+                    move2 = move2;
+                    SetGoal(8, move1, 1);
+                    SetGoal(16, move2, 1);
+                }
+                else if(strstr(buffer, "fanon"))
+                {
+                    printf("fan on\r\n");
+                    Fan.write(0.8);
+                }
+                else if(strstr(buffer, "fanoff"))    
+                {
+                    printf("fan off\r\n");
+                    Fan.write(0);
+                }
+            }
+        }
+}
+
+
+void SetGoal(int ID, int degrees, int flags) {
+
+    char reg_flag = 0;
+    char data[2];
+
+    // set the flag is only the register bit is set in the flag
+    if (flags == 0x2) {
+        reg_flag = 1;
+    }
+
+    // 1023 / 300 * degrees
+    int goal = degrees;
+    //short goal = (1023 * degrees) / 300;
+
+    data[0] = goal & 0xff; // bottom 8 bits
+    data[1] = goal >> 8;   // top 8 bits
+
+    // write the packet, return the error code
+    write(ID, AX12_REG_GOAL_POSITION, 2, data, reg_flag);
+    
+    if (flags == 1) {
+    // block until it comes to a halt
+    
+        while (isMoving(ID)) {}
+    }
+}
+
+int write(int ID, int start, int bytes, char* data, int flag) {
+// 0xff, 0xff, ID, Length, Intruction(write), Address, Param(s), Checksum
+
+    char TxBuf[16];
+    char sum = 0;
+    char Status[6];
+
+#ifdef AX12_WRITE_DEBUG
+    pc.printf("\nwrite(%d,0x%x,%d,data,%d)\n",ID,start,bytes,flag);
+#endif
+
+    // Build the TxPacket first in RAM, then we'll send in one go
+#ifdef AX12_WRITE_DEBUG
+    pc.printf("\nInstruction Packet\n  Header : 0xFF, 0xFF\n");
+#endif
+
+    TxBuf[0] = 0xff;
+    TxBuf[1] = 0xff;
+
+    // ID
+    TxBuf[2] = ID;
+    sum += TxBuf[2];
+
+#ifdef AX12_WRITE_DEBUG
+    pc.printf("  ID : %d\n",TxBuf[2]);
+#endif
+
+    // packet Length
+    TxBuf[3] = 3+bytes;
+    sum += TxBuf[3];
+
+#ifdef AX12_WRITE_DEBUG
+    pc.printf("  Length : %d\n",TxBuf[3]);
+#endif
+
+    // Instruction
+    if (flag == 1) {
+        TxBuf[4]=0x04;
+        sum += TxBuf[4];
+    } else {
+        TxBuf[4]=0x03;
+        sum += TxBuf[4];
+    }
+
+#ifdef AX12_WRITE_DEBUG
+    pc.printf("  Instruction : 0x%x\n",TxBuf[4]);
+#endif
+
+    // Start Address
+    TxBuf[5] = start;
+    sum += TxBuf[5];
+
+#ifdef AX12_WRITE_DEBUG
+    pc.printf("  Start : 0x%x\n",TxBuf[5]);
+#endif
+
+    // data
+    for (char i=0; i<bytes ; i++) {
+        TxBuf[6+i] = data[i];
+        sum += TxBuf[6+i];
+
+#ifdef AX12_WRITE_DEBUG
+        pc.printf("  Data : 0x%x\n",TxBuf[6+i]);
+#endif
+
+    }
+
+    // checksum
+    TxBuf[6+bytes] = 0xFF - sum;
+
+#ifdef AX12_WRITE_DEBUG
+    pc.printf("  Checksum : 0x%x\n",TxBuf[6+bytes]);
+#endif
+
+    // Transmit the packet in one burst with no pausing
+    for (int i = 0; i < (7 + bytes) ; i++) {
+        pc.putc(TxBuf[i]);
+    }
+        // Wait for the bytes to be transmitted
+    wait (0.00002);
+
+    // Skip if the read was to the broadcast address
+    if (ID != 0xFE) {
+
+        // response packet is always 6 + bytes
+        // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum
+        // timeout is a little more than the time to transmit
+        // the packet back, i.e. (6+bytes)*10 bit periods
+
+        int timeout = 0;
+        int plen = 0;
+        while ((timeout < ((6+bytes)*10)) && (plen<(6+bytes))) {
+
+            if (pc.readable()) {
+                Status[plen] = pc.getc();
+                plen++;
+                timeout = 0;
+            }
+
+            // wait for the bit period
+            wait (1.0/9600);
+            timeout++;
+        }
+
+        if (timeout == ((6+bytes)*10) ) {
+            return(-1);
+        }
+
+        // Copy the data from Status into data for return
+        for (int i=0; i < Status[3]-2 ; i++) {
+            data[i] = Status[5+i];
+        }
+
+#ifdef AX12_READ_DEBUG
+        printf("\nStatus Packet\n");
+        printf("  Header : 0x%x\n",Status[0]);
+        printf("  Header : 0x%x\n",Status[1]);
+        printf("  ID : 0x%x\n",Status[2]);
+        printf("  Length : 0x%x\n",Status[3]);
+        printf("  Error Code : 0x%x\n",Status[4]);
+
+        for (int i=0; i < Status[3]-2 ; i++) {
+            printf("  Data : 0x%x\n",Status[5+i]);
+        }
+
+        printf("  Checksum : 0x%x\n",Status[5+(Status[3]-2)]);
+#endif
+
+    } // if (ID!=0xFE)
+
+    return(Status[4]);
+}
+
+int read(int ID, int start, int bytes, char* data) {
+
+    char PacketLength = 0x4;
+    char TxBuf[16];
+    char sum = 0;
+    char Status[16];
+
+    Status[4] = 0xFE; // return code
+
+#ifdef AX12_READ_DEBUG
+    printf("\nread(%d,0x%x,%d,data)\n",ID,start,bytes);
+#endif
+
+    // Build the TxPacket first in RAM, then we'll send in one go
+#ifdef AX12_READ_DEBUG
+    printf("\nInstruction Packet\n  Header : 0xFF, 0xFF\n");
+#endif
+
+    TxBuf[0] = 0xff;
+    TxBuf[1] = 0xff;
+
+    // ID
+    TxBuf[2] = ID;
+    sum += TxBuf[2];
+
+#ifdef AX12_READ_DEBUG
+    printf("  ID : %d\n",TxBuf[2]);
+#endif
+
+    // Packet Length
+    TxBuf[3] = PacketLength;    // Length = 4 ; 2 + 1 (start) = 1 (bytes)
+    sum += TxBuf[3];            // Accululate the packet sum
+
+#ifdef AX12_READ_DEBUG
+    printf("  Length : 0x%x\n",TxBuf[3]);
+#endif
+
+    // Instruction - Read
+    TxBuf[4] = 0x2;
+    sum += TxBuf[4];
+
+#ifdef AX12_READ_DEBUG
+    printf("  Instruction : 0x%x\n",TxBuf[4]);
+#endif
+
+    // Start Address
+    TxBuf[5] = start;
+    sum += TxBuf[5];
+
+#ifdef AX12_READ_DEBUG
+    printf("  Start Address : 0x%x\n",TxBuf[5]);
+#endif
+
+    // Bytes to read
+    TxBuf[6] = bytes;
+    sum += TxBuf[6];
+
+#ifdef AX12_READ_DEBUG
+    printf("  No bytes : 0x%x\n",TxBuf[6]);
+#endif
+
+    // Checksum
+    TxBuf[7] = 0xFF - sum;
+#ifdef AX12_READ_DEBUG
+    printf("  Checksum : 0x%x\n",TxBuf[7]);
+#endif
+
+    // Transmit the packet in one burst with no pausing
+    for (int i = 0; i<8 ; i++) {
+        pc.putc(TxBuf[i]);
+    }
+
+    // Wait for the bytes to be transmitted
+    wait (0.00002);
+
+    // Skip if the read was to the broadcast address
+    if (ID != 0xFE) {
+
+
+
+        // response packet is always 6 + bytes
+        // 0xFF, 0xFF, ID, Length Error, Param(s) Checksum
+        // timeout is a little more than the time to transmit
+        // the packet back, i.e. (6+bytes)*10 bit periods
+
+        int timeout = 0;
+        int plen = 0;
+        while ((timeout < ((6+bytes)*10)) && (plen<(6+bytes))) {
+
+            if (pc.readable()) {
+                Status[plen] = pc.getc();
+                plen++;
+                timeout = 0;
+            }
+
+            // wait for the bit period
+            wait (1.0/9600);
+            timeout++;
+        }
+
+        if (timeout == ((6+bytes)*10) ) {
+            return(-1);
+        }
+
+        // Copy the data from Status into data for return
+        for (int i=0; i < Status[3]-2 ; i++) {
+            data[i] = Status[5+i];
+        }
+
+#ifdef AX12_READ_DEBUG
+        printf("\nStatus Packet\n");
+        printf("  Header : 0x%x\n",Status[0]);
+        printf("  Header : 0x%x\n",Status[1]);
+        printf("  ID : 0x%x\n",Status[2]);
+        printf("  Length : 0x%x\n",Status[3]);
+        printf("  Error Code : 0x%x\n",Status[4]);
+
+        for (int i=0; i < Status[3]-2 ; i++) {
+            printf("  Data : 0x%x\n",Status[5+i]);
+        }
+
+        printf("  Checksum : 0x%x\n",Status[5+(Status[3]-2)]);
+#endif
+
+    } // if (ID!=0xFE)
+
+    return(Status[4]);
+}
+
+int isMoving(int ID) {
+
+    char data[1];
+    read(ID,AX12_REG_MOVING,1,data);
+    return(data[0]);
+}
diff -r 87d959d6a3f3 -r 8d6d624124fd mbed-src.lib
--- a/mbed-src.lib	Tue Jul 14 06:55:18 2015 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,1 +0,0 @@
-https://developer.mbed.org/users/kaizen/code/mbed-src/#cf2abc059970
diff -r 87d959d6a3f3 -r 8d6d624124fd mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Wed Sep 23 00:50:09 2015 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/4f6c30876dfa
\ No newline at end of file