main.cpp

00001 #include "mbed.h"
00002 #include "GSwifiInterface.h"
00003 #include "Servo.h"
00004 
00005 Servo servo1(p22);
00006 Servo servo2(p23);
00007 
00008 Serial pc(USBTX, USBRX);
00009 
00010 GSwifiInterface gs(p13, p14, NC, NC, p9, "SSID", "PHRASE", GSwifi::SEC_WPA_PSK);
00011 // TX, RX, CTS, RTS, RESET
00012 
00013 unsigned char buff[30];
00014 int RecvCount = 0;
00015 int ParamLen = 0;
00016 int ParamCount = 0;
00017 void Recv(unsigned char data);
00018 void ServoCmdExec();
00019 
00020 int main () {
00021     pc.baud(115200);
00022     RecvCount = 0;
00023     ParamLen = 0;
00024     ParamCount = 0;
00025     servo1 = 0.5f;
00026     servo2 = 0.5f;
00027     pc.printf("OK");
00028 
00029     gs.init("192,168,1,17", "255,255,255,0", "192,168,1,254"); // use Static IP
00030     if (gs.connect() == false) {
00031         return -1; // join the network
00032     }
00033     printf("IP Address is %s\n\r", gs.getIPAddress());
00034     
00035     TCPSocketServer server;
00036     server.bind(10002);
00037     server.listen();
00038     
00039     while (true) {
00040         printf("\nWait for new connection...\n");
00041         TCPSocketConnection client;
00042         server.accept(client);
00043         client.set_blocking(false, 1500); // Timeout after (1.5)s
00044         
00045         printf("Connection from: %s\n", client.get_address());
00046         char buf[300];
00047         while (true) {
00048             int len = client.receive(buf, sizeof(buf));
00049             for (int i = 0; i < len; i++) {
00050                 Recv((unsigned char)buf[i]);
00051             }
00052         }
00053         
00054         client.close();
00055     }
00056 
00057     gs.disconnect();
00058 }
00059 
00060 void Recv(unsigned char data) {
00061     switch (RecvCount) {
00062         case 0:
00063             ParamLen = 0;
00064             ParamCount = 0;
00065             if (data == 0xff) { // Headder
00066                 buff[RecvCount++] = data;
00067             }
00068             break;
00069         case 1:
00070             if (data == 0) {    // ID
00071                 buff[RecvCount++] = data;
00072             }
00073             break;
00074         case 2:                 // LENGTH
00075             buff[RecvCount++] = data;
00076             ParamLen = data;
00077             break;
00078         default:
00079             if (ParamLen > 0) {
00080                 buff[RecvCount++] = data;
00081                 ParamCount++;
00082                 if (ParamCount >= ParamLen) {
00083                     ServoCmdExec();
00084                     RecvCount = 0;
00085                 }
00086             } else {
00087                 RecvCount = 0;
00088             }
00089             break;
00090     }
00091 }
00092 
00093 void ServoCmdExec() {
00094     float tmp1 = (float)buff[4]/255.0f;
00095     float tmp2 = (float)buff[5]/255.0f;
00096     servo1 = tmp1;
00097     servo2 = tmp2;
00098 
00099     pc.printf("servo1 = %02Xh %.1f",buff[4],tmp1);
00100     pc.printf("servo2 = %02Xh %.1f",buff[5],tmp2);
00101 }