程序如下,c/c++ 不怎么熟悉,求指教
- #include <Servo.h>
- #include <SoftwareSerial.h>
-
- #include "stdlib.h"
- #define ByteBufferLenght 64
-
- int E1 = 5; //M1 Speed Control
- int E2 = 6; //M2 Speed Control
- int M1 = 4; //M1 Direction Control
- int M2 = 7; //M1 Direction Control
-
- int gearPin1 = 9;
- int gearPin2 = 11;
-
- Servo servo1;
- Servo servo2;
-
- char ack = 'z';
- char flagServo1 = 'm';
- char flagServo2 = 'n';
- int bufferCount;
- char dataBuffer[ByteBufferLenght];
-
- boolean isInControl = true;
- boolean isStopped = true;
- String carStop = "stop";
-
- SoftwareSerial mySerial(12,10);
-
- void setup() {
- // put your setup code here, to run once:
- int i;
- for(i=4;i<=7;i++){
- pinMode(i, OUTPUT);
- }
- digitalWrite(E1,LOW);
- digitalWrite(E2,LOW);
-
- pinMode(12, INPUT);
- pinMode(10, OUTPUT);
- pinMode(gearPin1, OUTPUT);
- pinMode(gearPin2, OUTPUT);
-
- servo1.attach(gearPin1);
- servo2.attach(gearPin2);
-
- //set baud rate
- Serial.begin(38400);
- mySerial.begin(9600);
- }
-
- void loop() {
- // put your main code here, to run repeatedly:
- receiveData();
- delay(15);
- }
-
- void receiveData(){
- char lastByte;
- if(mySerial.available()>0){
- lastByte = mySerial.read();
- // if(lastByte>'z' || lastByte<'0'){
- // return;
- // }
- // Serial.print(lastByte);
- if(lastByte == ack){
- // Serial.println();
- processCommand();
- }
- else if(bufferCount < ByteBufferLenght){
- dataBuffer[bufferCount] = lastByte;
- bufferCount++;
- }
- }
- }
-
- void processCommand(){
- //processCommand
- char flag = dataBuffer[0];
- if(flag == 'c'){
- String command = getString();
- handleControlCommand(command);
- }
- else if(flag == 'l'){
- isStopped = false;
- int leftSpeed = getInt();
- left(leftSpeed);
- // Serial.print("l");
- // Serial.println(leftSpeed);
- }
- else if(flag == 'r'){
- isStopped = false;
- int rightSpeed = getInt();
- right(rightSpeed);
- // Serial.print("r");
- // Serial.println(rightSpeed);
- }
- else if(flag == flagServo1){
- int servo1Angle = getInt();
- Serial.print(flag);
- Serial.println(servo1Angle);
- servo1.write(servo1Angle);
- delay(15);
- }
- else if(flag == flagServo2){
- int servo2Angle = getInt();
- Serial.print(flag);
- Serial.println(servo2Angle);
- servo2.write(servo2Angle);
- delay(15);
- }
- reset();
- }
-
- int getInt()
- {
- char b[bufferCount-1];
- for(int a = 1;a < bufferCount;a++){
- b[a-1] = dataBuffer[a];
- }
-
- // b[bufferCount-1] = '\0';
- return atoi(b);
- }
-
- String getString(){
- String total;
- for(int a = 1;a < bufferCount;a++){
- total += String(dataBuffer[a]);
- }
- return total;
- }
-
- void reset(){
- for(int i = 0; i < ByteBufferLenght; i++){
- dataBuffer<i> = 0;
- }
- bufferCount = 0;
- }
-
- //when receive 'l' flag data, this function will be call
- void left(int leftSpeed){
- if(isInControl){
- //control left wheels
- if(leftSpeed > 0){
- //left wheels go forward
- analogWrite(E2,leftSpeed);
- digitalWrite(M2,HIGH);
- }
- else if(leftSpeed < 0){
- //left wheels go back
- analogWrite(E2,-leftSpeed);
- digitalWrite(M2,LOW);
- }
- else{
- //stop
- digitalWrite(E2,LOW);
- }
- }
- }
-
- //when receive 'r' flag data, this function will be call
- void right(int rightSpeed){
- if(isInControl){
- //control right wheels
- if(rightSpeed > 0){
- //right wheels go forward
- analogWrite(E1,rightSpeed);
- digitalWrite(M1,HIGH);
- }
- else if(rightSpeed < 0){
- //right wheels go back
- analogWrite(E1,-rightSpeed);
- digitalWrite(M1,LOW);
- }
- else{
- //stop
- digitalWrite(E1,LOW);
- }
- }
- }
-
- void handleControlCommand(String command){
- if(command == carStop && !isStopped){
- Serial.println("--hasStop--");
- // analogWrite(E1,0);
- // analogWrite(E2,0);
- isStopped = true;
- digitalWrite(E1,LOW);
- digitalWrite(E2,LOW);
- }
- }</i>
复制代码
|