/**************** wifi protocol ********************* 版权说明:Copyright 2022 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved. Distributed under MIT license.See file LICENSE for detail or copy at by 机器谱 2022-9-28https://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 <Servo.h> 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 = 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); } |
欢迎光临 机器人与人工智能爱好者论坛 (http://robot-ai.org/) | Powered by Discuz! X3.2 |