main.cpp

00001 #include "mbed.h"
00002 #include "Servo.h"
00003 #include "WizFi250Interface.h"
00004 
00005 #define SECURE WizFi250::SEC_WPA2_MIXED
00006 #define SSID "Kaizen_Skate"
00007 #define PASS "qwertyuiop"
00008 
00009 #define SERVER_PORT    5000
00010 #define STOP_MOTOR     50
00011 
00012 
00013 #if defined(TARGET_WIZwiki_W7500P)
00014     WizFi250Interface wizfi250(PC_2,PC_3,PC_13,PC_14,PC_12,NC,115200);
00015     Serial pc(USBTX, USBRX);
00016     Servo motor_speed(PC_8);      // it used to percentage value ( 0 ~ 1 )
00017     DigitalOut green_led(LED2);    
00018     DigitalOut red_led(LED1);
00019     DigitalOut blue_led(LED3);
00020 #endif
00021 
00022 #define START_MSG       "START"
00023 #define END_MSG         "END\n"
00024 #define CONTROL_UP      "UPXX"
00025 #define CONTROL_DOWN    "DOWN"
00026 #define CONTROL_CRUISER "CRUI"
00027 #define CONTROL_STOP    "STOP"
00028 
00029 #define STATUS_CMD_OK   "OKXX"
00030 #define STATUS_CMD_FAIL "FAIL"
00031 
00032 #define CONTROL_MSG_SIZE    19
00033 
00034 // Control Pkt Format : START,CONTROL,SPEED_VALUE,END
00035 //                      START,UPXX,500,END
00036 // Status Pkt Format : START,CONTROL,SPEED_VALUE,STATUS,END
00037 
00038 struct control_pkt{
00039     char start_msg[6];
00040     char control_msg[5];
00041     char speed_msg[4];
00042     char end_msg[4];
00043 };
00044 void parse(char buffer[], int *j, char *string);
00045 
00046 int main() {    
00047     int recv_cnt,j;
00048     volatile float speed_value = 0.5;
00049     char recv_control_msg[100];
00050     char status_msg[100];
00051     control_pkt control;
00052 
00053     pc.baud(115200);
00054     green_led = 1; red_led = 1; blue_led = 1;
00055 
00056     wizfi250.init();
00057     wizfi250.setAntMode(WizFi250::UFL);
00058     wizfi250.setAddress("192.168.100.2","255.255.255.0","192.168.100.1");
00059     if ( wizfi250.connect(SECURE, SSID, PASS, WizFi250::WM_AP))
00060     {
00061         red_led = 0;
00062         return -1;
00063     }
00064     printf("IP Address is %s\r\n", wizfi250.getIPAddress());
00065     green_led = 0; red_led = 0; blue_led = 0;
00066 
00067     TCPSocketServer server;
00068     TCPSocketConnection client;
00069 
00070     while(true)
00071     {
00072         if( client.is_connected() == false )
00073         {
00074             green_led = 1; red_led = 1; blue_led = 0;            
00075             client.close();
00076             server.close();
00077             
00078             if(speed_value == 0.5)
00079             {
00080                 server.bind(SERVER_PORT);
00081                 server.listen();
00082 
00083                 printf("\nWait for new connection...\r\n");
00084                 server.accept(client);
00085                 client.set_blocking(false, 1500);
00086             }
00087             else if(speed_value <= 0.4)
00088             {
00089                 printf("Speed decrease +\r\n");
00090 
00091                 speed_value = 0.5;
00092                 motor_speed = speed_value;
00093 //                wait(0.5);
00094 //                speed_value = 0.57;
00095 //                motor_speed = speed_value;
00096 //                wait(5);
00097 //                speed_value = 0.5;
00098 //                motor_speed = speed_value;
00099             }
00100             else
00101             {
00102                 printf("Speed decrease -\r\n");
00103                 speed_value = 0.5;  motor_speed = speed_value;
00104 //                wait(0.5);
00105 //                speed_value = 0.41; motor_speed = speed_value;
00106 //                wait(2);
00107 //                speed_value = 0.35; motor_speed = speed_value;
00108 //                wait(2);
00109 //                speed_value = 0.41; motor_speed = speed_value;
00110 //                wait(2);
00111 //                speed_value = 0.5;  motor_speed = speed_value;
00112 //                wait(2);
00113             }
00114         }
00115         else
00116         {
00117             motor_speed = speed_value;
00118 
00119             green_led = 0; red_led = 1; blue_led = 1;
00120             //recv_cnt = client.receive_all((char*)recv_control_msg, sizeof(control)-1);
00121             recv_cnt = client.receive_all((char*)recv_control_msg, sizeof(control));
00122             
00123             j=0;
00124             parse(recv_control_msg, &j, control.start_msg);
00125             parse(recv_control_msg, &j, control.control_msg);
00126             parse(recv_control_msg, &j, control.speed_msg);
00127             parse(recv_control_msg, &j, control.end_msg);
00128             
00129             if(recv_cnt > 0)
00130             {
00131                 if( (strncmp(control.start_msg,START_MSG,sizeof(control.start_msg)) != 0) || 
00132                     (strncmp(control.end_msg,END_MSG,sizeof(control.end_msg)) != 0) )
00133                 {
00134                     printf("TEST Error\r\n");
00135                     sprintf(status_msg,"%s,%03d,%s,%s\n",START_MSG,int(speed_value*100),STATUS_CMD_FAIL,END_MSG);
00136                     client.send_all((char*)status_msg,strlen(status_msg));
00137                     continue;
00138                 }
00139                 
00140                 if( strncmp(control.control_msg,CONTROL_UP,sizeof(control.control_msg)) == 0 )
00141                 {
00142                     speed_value += 0.05; 
00143                     motor_speed = speed_value;
00144                     printf("TEST UP %f\r\n",speed_value); 
00145                 }
00146                 else if( strncmp(control.control_msg,CONTROL_DOWN,sizeof(control.control_msg)) == 0)
00147                 {
00148                     speed_value -= 0.05; 
00149                     motor_speed = speed_value;
00150                     printf("TEST DOWN %f\r\n",speed_value);
00151                 }
00152                 else if( strncmp(control.control_msg,CONTROL_CRUISER,sizeof(control.control_msg)) == 0)
00153                 {
00154                     printf("TEST CRUISER\r\n"); 
00155                     speed_value = (float)(atoi(control.speed_msg)) / 100;
00156                     motor_speed = speed_value;
00157                 }
00158                 else if( strncmp(control.control_msg,CONTROL_STOP,sizeof(control.control_msg)) == 0)
00159                 {
00160                     printf("TEST STOP\r\n"); 
00161                     speed_value = 0.5;
00162                     motor_speed = speed_value;
00163                 }
00164                 else
00165                 {
00166                     printf("TEST Error 1-2\r\n");
00167                     sprintf(status_msg,"%s,%03d,%s,%s\n",START_MSG,int(speed_value*100),STATUS_CMD_FAIL,END_MSG);
00168                     client.send_all((char*)status_msg,strlen(status_msg));
00169                     continue;
00170                 }
00171 
00172                 sprintf(status_msg,"%s,%03d,%s,%s\n",START_MSG,int(speed_value*100),STATUS_CMD_OK,END_MSG);
00173                 client.send_all((char*)status_msg,strlen(status_msg));
00174             }
00175             else
00176             {
00177                 sprintf(status_msg,"%s,%03d,%s,%s\n",START_MSG,int(speed_value*100),STATUS_CMD_OK,END_MSG);
00178                 client.send_all((char*)status_msg,strlen(status_msg));
00179             }
00180         }
00181     }// while
00182 }
00183 
00184 void parse(char buffer[], int *j, char *string) {
00185 //extracts next location string data item from buffer
00186     int i=0;
00187     for (i=0; i<=strlen(buffer); i++) {  //TOTAL SIZE OF RETURNED DATA
00188         if ((buffer[*j+i] == ',')||(buffer[*j+i] == '\0' )) { //IF comma or end of string
00189             //comma is the string field delimiter
00190             string[i]=0; //SETS END OF SRTRING TO 0
00191             *j=*j+i+1; //UPDATES to 1 after comma seperated value
00192             break;
00193         } else string[i]=buffer[*j+i]; //Keep adding to the string
00194     }
00195 }