|
网上找的代码。控制时1号电机只能反转不能正转,2好电机正常,不知原因
#include <Servo.h>
Servo servo1;
Servo servo2;
int E1 = 5;
int E2 = 6;
int M1_1 =2;
int M1_2 =3;
int M2_1 = 4;
int M2_2 = 7;
int LED = 13;
int buffer[3];
int Serial_flag;
void setup()
{
pinMode(13,OUTPUT);
servo1.attach(9);//定义舵机控制口
servo2.attach(10);
Serial.begin(9600);
}
void Stop(void)
{
digitalWrite(E1,LOW);//程序可以控制两路舵机(9、10接口)+两路直流电机。
digitalWrite(E2,LOW);
}
void advance(int x)
{
analogWrite(E1,x);
digitalWrite(M1_1,LOW);
digitalWrite(M1_2,HIGH);
analogWrite(E2,x);
digitalWrite(M2_1,LOW);
digitalWrite(M2_2,HIGH);
}
void back(int x)
{
analogWrite(E1,x);
digitalWrite(M1_1,HIGH);
digitalWrite(M1_2,LOW);
analogWrite(E2,x);
digitalWrite(M2_1,HIGH);
digitalWrite(M2_2,LOW);
}
void left(int x)
{
analogWrite(E1,x);
digitalWrite(M1_1,LOW);
digitalWrite(M1_2,HIGH);
analogWrite(E2,x);
digitalWrite(M2_1,HIGH);
digitalWrite(M2_2,LOW);
}
void right(int x)
{
analogWrite(E1,x);
digitalWrite(M1_1,HIGH);
digitalWrite(M1_2,LOW);
analogWrite(E2,x);
digitalWrite(M2_1,LOW);
digitalWrite(M2_2,HIGH);
}
void Communication_Decode()
{
if(buffer[0]==0x00)
{
switch(buffer[1])
{
case 0x01:advance(150);return;
case 0x02:back(150);return;
case 0x03:right(150);return;
case 0x04:left(150);return;
case 0x00:Stop(); return;
}
}
else if(buffer[0]==0x01)
{
if(buffer[2]>180)return;
switch(buffer[1])
{
case 0x01:servo1.write(buffer[2]);return;
case 0x02:servo2.write(buffer[2]);return;
default:return;
}
}
}
void loop()
{
int temp_char;
int date_index=0;
while(1)
{
temp_char = Serial.read();
if(temp_char!=-1)
{
if(Serial_flag==0)
{
if(temp_char==0xff)
{
Serial_flag = 1;
}
}
else
{
if(temp_char==0xff)
{
Serial_flag = 0;
if(date_index==3)
{
Communication_Decode();
}
date_index = 0;
}
else
{
buffer[date_index]=temp_char;
Serial.println(buffer[date_index],HEX);
date_index++;
}
}
}
}
}
|
|