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;
+		}
+	}
+}