モータードライバとWi-FiモジュールESP-WROOM-02をmbed LPC1114FN28に繋げて、RCWControllerからコントロールするプログラム
Dependencies: mbed
Diff: main.cpp
- Revision:
- 0:3c24a40c2343
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Jul 22 05:36:02 2016 +0000 @@ -0,0 +1,171 @@ +#include "mbed.h" + +#include "ESP8266Interface.h" +#include "SoftSerialSendOnry.h" +#include "RCWController.h" + +/************************* WiFi Setting *********************************/ + +#define WLAN_SSID "wifi_car" +#define WLAN_PASS "wifi_car" + + +/************************* UDP Setting *********************************/ +#define SEND_PORT 10000 + +/************************* Port Class Setting *********************************/ +SoftSerialSendOnry pc(dp10); // tx +ESP8266Interface wifi(dp16,dp15,dp4,WLAN_SSID,WLAN_PASS); +PwmOut motor_pwm_a(dp24); +DigitalOut motor_sw_a(dp18); +PwmOut motor_pwm_b(dp1); +DigitalOut motor_sw_b(dp2); + +/************************* Variable Setting *********************************/ +char recv_buf[10]; +int recv_buf_p = 0; +int read_p = 0; +int recv_ana_phase = 0; + +/************************* Controller value to PWM vlaue *********************************/ +float Controll2Pwm(int value) +{ + return(((float)value/128.0f)-1.0f); +} + +/************************* Main func *********************************/ +int main() { + char* ip; + int recv_p = 0; + int timeout_count = 0; + + pc.printf("Start\r\n"); + + motor_pwm_a = 0.0; + motor_pwm_b = 0.0; + + wifi.init(); + pc.printf("Wifi init OK\r\n"); + + wifi.single_ap(); + //while(!wifi.connect()); + pc.printf("Wifi Connect OK\r\n"); + + ip = wifi.getIPAddress(); + if(ip != NULL) + { + pc.printf("IP:%s\r\n",ip); + } + else + { + pc.printf("IP:ERROR\r\n"); + } + + if(wifi.start(ESP_UDP_TYPE,ip, SEND_PORT) != false ) + { + pc.printf("UDP Port open OK\r\n"); + } + else + { + pc.printf("UDP Port open NG\r\n"); + } + + pc.printf("init All OK\r\n"); + + while(1) + { + while(wifi.readable()!=0) + { + recv_buf[recv_p] = wifi.getc(); + //pc.printf("recv[%d]:\r\n",recv_buf[recv_p]); + recv_p++; + + if(recv_p>=10) + { + float left,right; + RCWController *controller; +#if 0 + pc.printf("recv[%d]:",recv_p); + for(int i=0;i<recv_p;i++) + { + pc.printf("%02X ",recv_buf[i]); + } + pc.printf("\r\n"); +#endif + // Set Table + controller = (RCWController*)&recv_buf[0]; + + // Change Servo Value + if( (controller->status.LeftAnalogUD != 128)||(controller->status.RightAnalogUD != 128) ) + { + left = Controll2Pwm(controller->status.LeftAnalogUD); + right = Controll2Pwm(controller->status.RightAnalogUD); + + + } + else + { + if( controller->status.UP == 1 ) + { + left = 1.0f; + right = 1.0f; + } + else if( controller->status.DOWN == 1 ) + { + left = -1.0f; + right = -1.0f; + } + else if( controller->status.RIGHT == 1 ) + { + left = 1.0f; + right = -1.0f; + } + else if( controller->status.LEFT == 1 ) + { + left = -1.0f; + right = 1.0f; + } + else + { + left = 0.0f; + right = 0.0f; + } + } + + if(left > 0 ) + { + motor_pwm_a = left; + motor_sw_a = 1; + } + else + { + motor_pwm_a = -left; + motor_sw_a = 0; + } + if(right > 0 ) + { + motor_pwm_b = right; + motor_sw_b = 1; + } + else + { + motor_pwm_b = -right; + motor_sw_b = 0; + } + + //pc.printf("left = %f right = %f\r\n",left,right); + + recv_p = 0; + timeout_count = 0; + } + } + wait(0.005); + timeout_count++; + if( timeout_count > 1000 ) + { + // Time out Reset + timeout_count = 0; + recv_p = 0; + } + } +}