#include <ros.h>
#include <std_msgs/String.h>
#include <Servo.h>
//定义五中运动状态
#define STOP 0
#define FORWARD 1
#define BACKWARD 2
#define TURNLEFT 3
#define TURNRIGHT 4
//定义需要用到的引脚
int leftMotor1 = 4;
int leftMotor2 = 5;
int rightMotor1 = 6;
int rightMotor2 = 7;
ros::NodeHandle nh;
void messageCb(const std_msgs::String& cat_msg) {
switch(cat_msg.data[0]) {
case 'w':
motorRun(1);
delay(2000);//每个命令执行2s
motorRun(5);
break;
case 's':
motorRun(2);
delay(2000);//每个命令执行2s
motorRun(5);
break;
case 'a':
motorRun(3);
delay(2000);//每个命令执行2s
motorRun(5);
break;
case 'd':
motorRun(4);
delay(2000);//每个命令执行2s
motorRun(5);
break;
}
}
ros::Subscriber<std_msgs::String> sub("cat_run", &messageCb);
void setup() {
// put your setup code here, to run once:
//设置控制电机的引脚为输出状态
pinMode(leftMotor1, OUTPUT);
pinMode(leftMotor2, OUTPUT);
pinMode(rightMotor1, OUTPUT);
pinMode(rightMotor2, OUTPUT);
nh.initNode();
nh.subscribe(sub);
}
void loop() {
// put your main code here, to run repeatedly:
nh.spinOnce();
delay(1);
}
//运动控制函数
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:
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, HIGH);
break;
case TURNRIGHT:
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);
}
}
#include <std_msgs/String.h>
#include <Servo.h>
//定义五中运动状态
#define STOP 0
#define FORWARD 1
#define BACKWARD 2
#define TURNLEFT 3
#define TURNRIGHT 4
//定义需要用到的引脚
int leftMotor1 = 4;
int leftMotor2 = 5;
int rightMotor1 = 6;
int rightMotor2 = 7;
ros::NodeHandle nh;
void messageCb(const std_msgs::String& cat_msg) {
switch(cat_msg.data[0]) {
case 'w':
motorRun(1);
delay(2000);//每个命令执行2s
motorRun(5);
break;
case 's':
motorRun(2);
delay(2000);//每个命令执行2s
motorRun(5);
break;
case 'a':
motorRun(3);
delay(2000);//每个命令执行2s
motorRun(5);
break;
case 'd':
motorRun(4);
delay(2000);//每个命令执行2s
motorRun(5);
break;
}
}
ros::Subscriber<std_msgs::String> sub("cat_run", &messageCb);
void setup() {
// put your setup code here, to run once:
//设置控制电机的引脚为输出状态
pinMode(leftMotor1, OUTPUT);
pinMode(leftMotor2, OUTPUT);
pinMode(rightMotor1, OUTPUT);
pinMode(rightMotor2, OUTPUT);
nh.initNode();
nh.subscribe(sub);
}
void loop() {
// put your main code here, to run repeatedly:
nh.spinOnce();
delay(1);
}
//运动控制函数
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:
digitalWrite(leftMotor1, HIGH);
digitalWrite(leftMotor2, LOW);
digitalWrite(rightMotor1, LOW);
digitalWrite(rightMotor2, HIGH);
break;
case TURNRIGHT:
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);
}
}