83 lines
1.9 KiB
C++
83 lines
1.9 KiB
C++
#include <Servo.h>
|
|
|
|
//定义五中运动状态
|
|
#define STOP 0 // 停止
|
|
#define FORWARD 1 // 前进
|
|
#define BACKWARD 2 // 后退
|
|
#define TURNLEFT 3 // 左转
|
|
#define TURNRIGHT 4 // 右转
|
|
|
|
|
|
//定义需要用到的引脚
|
|
int leftMotor1 = 16; // D0 红线
|
|
int leftMotor2 = 5; // D1 褐线
|
|
int leftPWM = 4;
|
|
|
|
int rightMotor1 = 0; // D3 黑线
|
|
int rightMotor2 = 2; // D4 白线
|
|
int rightPWM = 14;
|
|
|
|
Servo servo;
|
|
void setup() {
|
|
//设置控制电机的引脚为输出状态
|
|
pinMode(leftMotor1, OUTPUT);
|
|
pinMode(leftMotor2, OUTPUT);
|
|
pinMode(rightMotor1, OUTPUT);
|
|
pinMode(rightMotor2, OUTPUT);
|
|
|
|
pinMode(leftPWM, OUTPUT);
|
|
pinMode(rightPWM, OUTPUT);
|
|
|
|
servo.attach(13); //D7
|
|
servo.write(90);
|
|
}
|
|
|
|
void loop() {
|
|
int cmd;
|
|
for(cmd=0;cmd<5;cmd++) {
|
|
analogWrite(leftPWM, 100);
|
|
analogWrite(rightPWM, 100);
|
|
motorRun(cmd);
|
|
delay(2000);
|
|
}
|
|
}
|
|
|
|
|
|
//运动控制函数
|
|
void motorRun(int cmd)
|
|
{
|
|
switch(cmd){
|
|
case FORWARD:
|
|
digitalWrite(leftMotor1, LOW);
|
|
digitalWrite(leftMotor2, HIGH);
|
|
digitalWrite(rightMotor1, LOW);
|
|
digitalWrite(rightMotor2, HIGH);
|
|
break;
|
|
case BACKWARD:
|
|
digitalWrite(leftMotor1, HIGH);
|
|
digitalWrite(leftMotor2, LOW);
|
|
digitalWrite(rightMotor1, HIGH);
|
|
digitalWrite(rightMotor2, LOW);
|
|
break;
|
|
case TURNLEFT:
|
|
servo.write(0);
|
|
// digitalWrite(leftMotor1, HIGH);
|
|
// digitalWrite(leftMotor2, LOW);
|
|
// digitalWrite(rightMotor1, LOW);
|
|
// digitalWrite(rightMotor2, HIGH);
|
|
break;
|
|
case TURNRIGHT:
|
|
servo.write(180);
|
|
// digitalWrite(leftMotor1, LOW);
|
|
// digitalWrite(leftMotor2, HIGH);
|
|
// digitalWrite(rightMotor1, HIGH);
|
|
// digitalWrite(rightMotor2, LOW);
|
|
break;
|
|
default:
|
|
digitalWrite(leftMotor1, LOW);
|
|
digitalWrite(leftMotor2, LOW);
|
|
digitalWrite(rightMotor1, LOW);
|
|
digitalWrite(rightMotor2, LOW);
|
|
}
|
|
}
|