好的,谢谢,
下面是完整代码,请大神救急
int L_IN1 = 7; // IN1:0 IN2:1 正转
int L_IN2 = 5; // IN1:1 IN2:0 反转
int L_IN3 = 4;
int L_IN4 = 2;
int L_ENA = 6; //PWM输出口
int L_ENB = 3; //PWM输出口
int R_ENA = 13; //PWM输出口
int R_IN1 = 12; // IN1:0 IN2:1 正转
int R_IN2 = 11; // IN1:1 IN2:0 反转
int R_IN3 = 10;
int R_IN4 = 9;
int R_ENB = 8; //PWM输出口
int MOTO_LF_EN = 22;
int MOTO_RF_EN = 23;
int MOTO_LB_EN = 24;
int MOTO_RB_EN = 25;
int FRONT;
int BACK;
int LEFT;
int RIGHT;
int LF;
int RF;
int LB;
int RB;
void setup() {
// put your setup code here, to run once:
pinMode(L_IN1, OUTPUT);
pinMode(L_IN2, OUTPUT);
pinMode(L_IN3, OUTPUT);
pinMode(L_IN4, OUTPUT);
pinMode(L_ENA, OUTPUT);
pinMode(L_ENB, OUTPUT);
pinMode(R_IN1, OUTPUT);
pinMode(R_IN2, OUTPUT);
pinMode(R_IN3, OUTPUT);
pinMode(R_IN4, OUTPUT);
pinMode(R_ENA, OUTPUT);
pinMode(R_ENB, OUTPUT);
pinMode(MOTO_LF_EN, INPUT);
pinMode(MOTO_RF_EN, INPUT);
pinMode(MOTO_LB_EN, INPUT);
pinMode(MOTO_RB_EN, INPUT);
}
void forward() //正转
{
digitalWrite(L_ENA, HIGH);
digitalWrite(L_ENB, HIGH);
//analogWrite(ENA, 220);
//analogWrite(ENB, 220);
digitalWrite(R_ENA, HIGH);
digitalWrite(R_ENB, HIGH);
//analogWrite(ENA, 220);
//analogWrite(ENB, 220);
digitalWrite(L_IN1, LOW);
digitalWrite(L_IN2, HIGH);
digitalWrite(L_IN3, HIGH);
digitalWrite(L_IN4, LOW);
digitalWrite(R_IN1, HIGH);
digitalWrite(R_IN2, LOW);
digitalWrite(R_IN3, LOW);
digitalWrite(R_IN4, HIGH);
}
void reverse() //反转
{
digitalWrite(L_ENA, HIGH);
digitalWrite(L_ENB, HIGH);
//analogWrite(ENA, 220);
//analogWrite(ENB, 220);
digitalWrite(R_ENA, HIGH);
digitalWrite(R_ENB, HIGH);
//analogWrite(ENA, 220);
//analogWrite(ENB, 220);
digitalWrite(L_IN1, HIGH);
digitalWrite(L_IN2, LOW);
digitalWrite(L_IN3, LOW);
digitalWrite(L_IN4, HIGH);
digitalWrite(R_IN1, LOW);
digitalWrite(R_IN2, HIGH);
digitalWrite(R_IN3, HIGH);
digitalWrite(R_IN4, LOW);
}
void turn_left()
{
digitalWrite(L_ENA, HIGH);
digitalWrite(L_ENB, HIGH);
//analogWrite(ENA, 220);
//analogWrite(ENB, 220);
digitalWrite(R_ENA, HIGH);
digitalWrite(R_ENB, HIGH);
//analogWrite(ENA, 220);
//analogWrite(ENB, 220);
digitalWrite(L_IN1, HIGH);
digitalWrite(L_IN2, LOW);
digitalWrite(L_IN3, LOW);
digitalWrite(L_IN4, HIGH);
digitalWrite(R_IN1, HIGH);
digitalWrite(R_IN2, LOW);
digitalWrite(R_IN3, LOW);
digitalWrite(R_IN4, HIGH);
}
void turn_right()
{
digitalWrite(L_ENA, HIGH);
digitalWrite(L_ENB, HIGH);
//analogWrite(ENA, 220);
//analogWrite(ENB, 220);
digitalWrite(R_ENA, HIGH);
digitalWrite(R_ENB, HIGH);
//analogWrite(ENA, 220);
//analogWrite(ENB, 220);
digitalWrite(L_IN1, LOW);
digitalWrite(L_IN2, HIGH);
digitalWrite(L_IN3, HIGH);
digitalWrite(L_IN4, LOW);
digitalWrite(R_IN1, LOW);
digitalWrite(R_IN2, HIGH);
digitalWrite(R_IN3, HIGH);
digitalWrite(R_IN4, LOW);
}
void stop_car() //电机自由停止状态
{
digitalWrite(L_ENA, LOW);
digitalWrite(L_ENB, LOW);
//analogWrite(ENA, 220);
//analogWrite(ENB, 220);
digitalWrite(R_ENA, LOW);
digitalWrite(R_ENB, LOW);
//analogWrite(ENA, 220);
//analogWrite(ENB, 220);
digitalWrite(L_IN1, LOW);
digitalWrite(L_IN2, LOW);
digitalWrite(L_IN3, LOW);
digitalWrite(L_IN4, LOW);
digitalWrite(R_IN1, LOW);
digitalWrite(R_IN2, LOW);
digitalWrite(R_IN3, LOW);
digitalWrite(R_IN4, LOW);
}
void brake() //电机制动
{
digitalWrite(L_ENA, HIGH);
digitalWrite(L_ENB, HIGH);
//analogWrite(ENA, 220);
//analogWrite(ENB, 220);
digitalWrite(R_ENA, HIGH);
digitalWrite(R_ENB, HIGH);
//analogWrite(ENA, 220);
//analogWrite(ENB, 220);
digitalWrite(L_IN1, LOW);
digitalWrite(L_IN2, LOW);
digitalWrite(L_IN3, LOW);
digitalWrite(L_IN4, LOW);
digitalWrite(R_IN1, LOW);
digitalWrite(R_IN2, LOW);
digitalWrite(R_IN3, LOW);
digitalWrite(R_IN4, LOW);
}
int data_reading() //读取 SLAMWARE breakout 3 板子 电机控制板的信号
{
LF = digitalRead(MOTO_LF_EN); //电机的使能信号,低电平有效
RF = digitalRead(MOTO_RF_EN);
LB = digitalRead(MOTO_LB_EN);
RB = digitalRead(MOTO_RB_EN);
if ((LF == LOW) && (RF == LOW) && (LB == HIGH) && (RB == HIGH)) //前进
{
return FRONT;
}
else if ((LF == LOW) && (RF == HIGH) && (LB == HIGH) && (RB == LOW)) //右转
{
return RIGHT;
}
else if ((LF == HIGH) && (RF == LOW) && (LB == LOW) && (RB == HIGH)) //左转
{
return LEFT;
}
else if ((LF == HIGH) && (RF == HIGH) && (LB == LOW) && (RB == LOW)) //后退
{
return BACK;
}
}
void loop() {
// put your main code here, to run repeatedly:
switch (data_reading())
{
case FRONT:
forward();
break;
case BACK:
reverse();
break;
case LEFT:
turn_left();
break;
case RIGHT:
turn_right();
break;
// case stop_car:
// car_stop();
// break;
default:
brake();
break;
}
} |