/**************** wifi protocol *********************

版權說明：Copyright 2022 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.

Distributed under MIT license.See file LICENSE for detail or copy at

https://opensource.org/licenses/MIT

by 機器譜 2022-9-28 https://www.robotway.com/

通信協議：

* ff 00 00 00 ff

控制停止的WiFi通信協議

* ff 00 01 00 ff

控制前進的WiFi通信協議

* ff 00 02 00 ff

控制后退的WiFi通信協議

* ff 00 03 00 ff

控制左轉的WiFi通信協議

* ff 00 04 00 ff

控制右轉的WiFi通信協議

****************************************************/

#include

int moveDirection; // 1-forward, 2-backward, 3-left, 4-right, 5-stop

int inputData[4]; //data from wifi

void setup()

{

Serial.begin(9600);

}

void loop()

{

WiFi_Rescue_Robot();

}

void WiFi_Rescue_Robot()

{

WifiControl();

Move(moveDirection);

}

void WifiControl()

{

//operate wheel

if(inputData[0] == 0x00)

{

if(inputData[1] != moveDirection)

{

moveDirection = inputData[1];

}

}

}

void serialEvent()

{

static int i;

static boolean revStart;

while (Serial.available())

{

//get data from wifi

int inData = Serial.read();

if (inData == 0xff && !revStart)

{

revStart = true;

}

else if(revStart)

{

inputData[i] = inData;

i++;

if(i > 3)

{

if(inputData[3] == 0xff)

{

i = 0;

revStart = false;

}

else

{

i = 0;

revStart = false;

}

}

}

}

}

void Move(int direction)

{

switch (direction)

{

case 0x01:MoveForward();break;

case 0x02:MoveBackward();break;

case 0x03:MoveLeft();break;

case 0x04:MoveRight();break;

case 0x05:MoveStop();break;

default:MoveStop();break;

}

}

void MoveForward()

{

//motor_0 clockwise

digitalWrite(5, LOW);

digitalWrite(6, HIGH);

//motor_1 clockwise

digitalWrite(9, LOW);

digitalWrite(10, HIGH);

}

void MoveBackward()

{

//motor_0 clockwise

digitalWrite(5, HIGH);

digitalWrite(6, LOW);

//motor_1 clockwise

digitalWrite(9, HIGH);

digitalWrite(10, LOW);

}

void MoveRight()

{

//motor_0 clockwise

digitalWrite(5, HIGH);

digitalWrite(6, LOW);

//motor_1 clockwise

digitalWrite(9, LOW);

digitalWrite(10, HIGH);

}

void MoveLeft()

{

//motor_0 clockwise

digitalWrite(5, LOW);

digitalWrite(6, HIGH);

//motor_1 clockwise

digitalWrite(9, HIGH);

digitalWrite(10, LOW);

}

void MoveStop()

{

//motor_0 clockwise

digitalWrite(5, HIGH);

digitalWrite(6, HIGH);

//motor_1 clockwise

digitalWrite(9, HIGH);

digitalWrite(10, HIGH);

}