main.cpp

00001 #include "mbed.h"
00002 
00003 #include "ESP8266Interface.h"
00004 #include "SoftSerialSendOnry.h"
00005 #include "RCWController.h"
00006 
00007 /************************* WiFi Setting *********************************/
00008 
00009 #define WLAN_SSID       "wifi_car"
00010 #define WLAN_PASS       "wifi_car"
00011 
00012 
00013 /************************* UDP Setting *********************************/
00014 #define SEND_PORT       10000
00015 
00016 /************************* Port Class Setting *********************************/
00017 SoftSerialSendOnry pc(dp10); // tx
00018 ESP8266Interface    wifi(dp16,dp15,dp4,WLAN_SSID,WLAN_PASS);
00019 PwmOut      motor_pwm_a(dp24);
00020 DigitalOut  motor_sw_a(dp18);
00021 PwmOut      motor_pwm_b(dp1);
00022 DigitalOut  motor_sw_b(dp2);
00023 
00024 /************************* Variable Setting *********************************/
00025 char recv_buf[10];
00026 int recv_buf_p = 0;
00027 int read_p = 0;
00028 int recv_ana_phase = 0;
00029 
00030 /************************* Controller value to PWM vlaue *********************************/
00031 float Controll2Pwm(int value)
00032 {
00033     return(((float)value/128.0f)-1.0f);
00034 }
00035 
00036 /************************* Main func *********************************/
00037 int main() {
00038     char* ip;
00039     int recv_p = 0;
00040     int timeout_count = 0;
00041 
00042     pc.printf("Start\r\n");
00043     
00044     motor_pwm_a = 0.0;
00045     motor_pwm_b = 0.0;
00046     
00047     wifi.init();
00048     pc.printf("Wifi init OK\r\n");
00049     
00050     wifi.single_ap();
00051     //while(!wifi.connect());
00052     pc.printf("Wifi Connect OK\r\n");
00053     
00054     ip = wifi.getIPAddress();
00055     if(ip != NULL)
00056     {
00057         pc.printf("IP:%s\r\n",ip);
00058     }
00059     else
00060     {
00061         pc.printf("IP:ERROR\r\n");
00062     }
00063     
00064     if(wifi.start(ESP_UDP_TYPE,ip, SEND_PORT) != false )
00065     {
00066         pc.printf("UDP Port open OK\r\n");  
00067     }
00068     else
00069     {
00070         pc.printf("UDP Port open NG\r\n");      
00071     }
00072         
00073     pc.printf("init All OK\r\n");
00074     
00075     while(1)
00076     {
00077         while(wifi.readable()!=0)
00078         {
00079             recv_buf[recv_p] = wifi.getc(); 
00080             //pc.printf("recv[%d]:\r\n",recv_buf[recv_p]);
00081             recv_p++;
00082             
00083             if(recv_p>=10)
00084             {
00085                 float left,right;
00086                 RCWController *controller;
00087 #if 0
00088                 pc.printf("recv[%d]:",recv_p);
00089                 for(int i=0;i<recv_p;i++)
00090                 {
00091                     pc.printf("%02X ",recv_buf[i]);
00092                 }
00093                 pc.printf("\r\n");
00094 #endif
00095                 // Set Table
00096                 controller = (RCWController*)&recv_buf[0];
00097                 
00098                 // Change Servo Value
00099                 if( (controller->status.LeftAnalogUD != 128)||(controller->status.RightAnalogUD != 128) )
00100                 {
00101                     left = Controll2Pwm(controller->status.LeftAnalogUD);
00102                     right = Controll2Pwm(controller->status.RightAnalogUD);
00103                     
00104                     
00105                 }
00106                 else
00107                 {
00108                     if( controller->status.UP == 1 )
00109                     {
00110                         left = 1.0f;
00111                         right = 1.0f;
00112                     }
00113                     else if( controller->status.DOWN == 1 )
00114                     {
00115                         left = -1.0f;
00116                         right = -1.0f;
00117                     }
00118                     else if( controller->status.RIGHT == 1 )
00119                     {
00120                         left = 1.0f;
00121                         right = -1.0f;
00122                     }
00123                     else if( controller->status.LEFT == 1 )
00124                     {
00125                         left = -1.0f;
00126                         right = 1.0f;
00127                     }
00128                     else
00129                     {
00130                         left = 0.0f;
00131                         right = 0.0f;
00132                     }
00133                 }
00134                 
00135                 if(left > 0 )
00136                 {
00137                     motor_pwm_a = left;
00138                     motor_sw_a = 1;
00139                 }
00140                 else
00141                 {
00142                     motor_pwm_a = -left;
00143                     motor_sw_a = 0;         
00144                 }
00145                 if(right > 0 )
00146                 {
00147                     motor_pwm_b = right;
00148                     motor_sw_b = 1;
00149                 }
00150                 else
00151                 {
00152                     motor_pwm_b = -right;
00153                     motor_sw_b = 0;         
00154                 }
00155                                 
00156                 //pc.printf("left = %f  right = %f\r\n",left,right);
00157                 
00158                 recv_p = 0;
00159                 timeout_count = 0;
00160             }
00161         }
00162         wait(0.005);
00163         timeout_count++;
00164         if( timeout_count > 1000 )
00165         {
00166             // Time out Reset
00167             timeout_count = 0;
00168             recv_p = 0;
00169         }
00170     }
00171 }