RCWController.ino

#include <Wire.h>
#include <ESP8266WiFi.h>
#include <WiFiUdp.h>

const char ssid[] = "ESP8266";
const char pass[] = "password";

#define DEBUG	Serial
#define ON_BOARD_LED	14

#define DRV8830_A1_A0_0_0		0b1100000
#define DRV8830_A1_A0_0_open	0b1100001
#define DRV8830_A1_A0_0_1		0b1100010
#define DRV8830_A1_A0_open_0	0b1100011
#define DRV8830_A1_A0_open_open	0b1100100
#define DRV8830_A1_A0_open_1	0b1100101
#define DRV8830_A1_A0_1_0		0b1100110
#define DRV8830_A1_A0_1_open	0b1100111
#define DRV8830_A1_A0_1_1		0b110100

const int motorL = DRV8830_A1_A0_open_open;
const int motorR = DRV8830_A1_A0_open_1;

const int PACKET_SIZE = 256;

WiFiUDP udp;
unsigned int localPort = 10000;
char packetBuffer[PACKET_SIZE];
int status = WL_IDLE_STATUS;
int prev_X = 128, prev_Y = 128;
bool isChange = false;
int ledState = LOW;
int speedL = 0, speedR = 0;

bool isDebugOut = false;

int DRV8830_setSpeed(int DRV8830_Address, int speed)
{
	byte direction;
	byte power;
	if (speed == 0)
	{
		direction = 0x00;
	}
	else if (speed < 0)
	{
		direction = 0b10;
		speed = -1 * speed;
	}
	else
	{
		direction = 0b01;
	}

	power = (byte)(speed & 0xff);

	if (power > 0x3f)
	{

		power = 0x3f;
	}

	speed = power;

	power = (power << 2) | direction;

	Wire.beginTransmission(DRV8830_Address);
	Wire.write(0x00);
	Wire.write(power);
	Wire.endTransmission();

	if (direction = 0b10)
	{
		speed = -1 * speed;
	}

	return(speed);
}

void DRV8830_brake(int DRV8830_Addres)
{
	Wire.beginTransmission(DRV8830_Addres);
	Wire.write(0x00);
	Wire.write(0x03);
	Wire.endTransmission();
}

int DRV8830_getSpeed(int DRV8830_Addres)
{
	int speed = 0;
	byte data;

	Wire.beginTransmission(DRV8830_Addres);
	Wire.write(0x00);
	Wire.endTransmission();
	Wire.requestFrom((int)DRV8830_Addres, 1);
	while (Wire.available() == 0);
	data = Wire.read();
	if ((data & 0x03) == 0b10)
	{
		speed = -1 * (data >> 2);
	}
	else
	{
		speed = data >> 2;
	}

	return speed;
}


void setup()
{
	DEBUG.begin(115200);
	while (!DEBUG)
	{
		;
	}

	DEBUG.println("program start..");

	Wire.begin();
	DRV8830_setSpeed(motorL, 0);
	DRV8830_setSpeed(motorR, 0);

	WiFi.softAP(ssid, pass);
	IPAddress myIP = WiFi.softAPIP();

	DEBUG.print("AP IP address: ");
	DEBUG.println(myIP);

	DEBUG.println("Starting UDP");
	udp.begin(localPort);
	DEBUG.print("Local port: ");
	DEBUG.println(udp.localPort());
}

void loop()
{
	int rlen, val_X = 0, val_Y = 128;

	while (true)
	{
		rlen = udp.parsePacket();
		if (rlen < 10) {
			delay(1);
			continue;
		}

		udp.read(packetBuffer, (rlen > PACKET_SIZE) ? PACKET_SIZE : rlen);

		val_X = packetBuffer[2];
		if (val_X != prev_X)
		{
			prev_X = val_X;
			isChange = true;
			isDebugOut = true;
		}

		val_Y = packetBuffer[3];
		if (val_Y != prev_Y)
		{
			prev_Y = val_Y;
			isChange = true;
			isDebugOut = true;
		}

		if (isChange) {
			int speed = (val_Y - 128);
			int direction = (val_X - 128);
			speedL = speed + direction;
			speedR = speed - direction;
			DRV8830_setSpeed(motorL, speedL);
			DRV8830_setSpeed(motorR, speedR);
			isChange = false;
		}
		
		delay(1);

		if (isDebugOut) {
			DEBUG.print(val_X);
			DEBUG.print(" , ");
			DEBUG.print(val_Y);
			DEBUG.print(" : ");
			DEBUG.print(speedL);
			DEBUG.print(" , ");
			DEBUG.println(speedR);
			isDebugOut = false;
		}

	}
}