太极创客的项目太乐1号原始地址：http://www.taichi-maker.com/homepage/arduino-tutorial-index/arduino-hardware/#taile1
本文修改后的源代码下载地址：https://download.csdn.net/download/acktomas/16633001
关于修改过程中类的引用的文章可参考：C++实例：类的组合，即在一个类中包含另一个类的对象
本文对太乐1号项目中的Arduino代码和库文件进行了一些修正，解决了太乐1号中舵机抖动，我初步查看造成抖动的原因是雷达传感器对舵机信号的干扰，将太乐1号中servo库的引用去掉，加入了newPing库，关于舵机抖动的解决方法，可以参考我的博文：舵机抖动的解决方法
arduino代码
#include <Tyler_2.h>
#include <SoftwareSerial.h>
#include <NewPing.h>
#define OK_DIST 25 // 避障距离参数（cm）
#define MAN 0 // 手动模式
#define AUTO 1 // 自动模式
#define GEST 2 // 体感模式
#define TURN_LEFT_90 500 // 左转90度延迟参数
#define TURN_RIGHT_90 500 // 右转90度延迟参数
#define TURN_BACK 1600 // 掉头延迟参数
#define MAX_SPEED 200
#define ECHO_PIN A0
#define TRIG_PIN A1
#define MAX_DISTANCE 200
#define SERVO_PIN 10
#define BLUE_TOOTH_PIN 9
// 建立SoftwareSerial对象，HC-06的TX接Arduino引脚9（AFMOTOR SERVO-2引脚）
SoftwareSerial softSerial(BLUE_TOOTH_PIN, 2);
char cmdChar = '5'; // 存放串口控制指令字符
char cmdCharSave = cmdChar; // 串口控制指令缓存
byte systemMode = MAN; // 用于控制系统运行模式
// 建立太乐1号对象。其中对象参数分别是：
// (车轮电机1运转方向, 车轮电机2运转方向, 车轮电机3运转方向, 车轮电机4运转方向,
// 车轮电机运转速度，测距传感器TRIG引脚， 测距传感器ECHO引脚，最大测距距离 )
Tyler_2 tyler_2(1, 1, 1, 1, 200,TRIG_PIN,ECHO_PIN,SERVO_PIN,MAX_DISTANCE);
void setup() {
Serial.begin(9600); // 启动硬件串口（用于输出系统调试信息和接收电脑控制指令）
delay(100);
tyler_2.setHeadPos(90);
delay(1000);
// Serial.println(F("***************************"));
}
void loop() {
if (softSerial.available()) { // 如果软件串口收到信息
cmdChar = softSerial.read(); // 将信息传递给cmdChar变量
// Serial.print("cmdChar = "); // 硬件串口输出cmdChar变量信息，
// Serial.println(cmdChar); // 以便于开发调试使用
}
// if (Serial.available()) { // 如果硬件串口收到信息
// cmdChar = Serial.read(); // 将信息传递给cmdChar变量
// Serial.print("cmdChar = "); // 硬件串口输出cmdChar变量信息，
// Serial.println(cmdChar); // 以便于开发调试使用
// }
runMode(); // 执行运行模式并实施相应控制
}
// 执行运行模式并实施相应控制
void runMode() {
switch (cmdChar) {
case 'A': // 用户输入控制指令字符为自动运行字符'A'
systemMode = AUTO; // 将当前运行模式控制变量设置为AUTO
break;
case 'M': // 用户输入控制指令字符为自动运行字符'M'
systemMode = MAN; // 将当前运行模式控制变量设置为手动
tyler_2.stop(); // 切换为手动模式后自动停车
cmdChar = '5'; // 并且将控制指令字符设置为'5'（停车字符）
break;
case 'G': // 用户输入控制指令字符为自动运行字符'G'
systemMode = GEST; // 将当前运行模式控制变量设置为体感模式
tyler_2.stop(); // 切换为体感模式后自动停车
cmdChar = '5'; // 并且将控制指令字符设置为'5'（停车字符）
break;
}
if (systemMode == MAN || systemMode == GEST ) { // 如果当前运行模式为手动或体感
manMode(); // 则使用手动控制函数控制太乐1号（体感和手动模式的控制字符相同）
} else if (systemMode == AUTO) { // 如果当前运行模式为自动
autoMode(); // 则使用自动控制函数控制太乐1号
}
// modeReport(); // 输出运行模式信息
}
// 太乐1号手动控制函数
void manMode() {
switch (cmdChar) {
case '8': // 前进
tyler_2.forward();
break;
case '2': // 后退
tyler_2.backward();
break;
case '4': //左转
tyler_2.turnL();
break;
case '6': //右转
tyler_2.turnR();
break;
case '5': //停止
tyler_2.stop();
break;
case '7': //左前
tyler_2.forwardL();
break;
case '9': //右前
tyler_2.forwardR();
break;
case '1': //右后
tyler_2.backwardR();
break;
case '3': //左后
tyler_2.backwardL();
break;
}
}
// 太乐1号自动控制函数
void autoMode() {
if (cmdChar == '5') { // 如果控制字符为'5'停止
tyler_2.stop(); // 太乐1号停止
tyler_2.setHeadPos(90); // 头部舵机恢复向前
delay(50);
} else { // 如果控制字符不是'5'停止
delay(50); // 提高系统稳定性等待
int frontDist = tyler_2.getDistance(); // 检查前方距离
if (frontDist >= OK_DIST) { // 如果检测前方距离读数大于等于允许距离参数
tyler_2.forward(); // 太乐1号向前
} else { // 如果检测前方距离小于允许距离参数
tyler_2.stop(); // 太乐1号停止
// autoTurn(); // 检测左右侧距离并做出自动转向
int leftDistance = look(175);
tyler_2.setHeadPos(90); // 头部舵机恢复向前
delay(100);
int rightDistance = look(5);
tyler_2.setHeadPos(90); // 头部舵机恢复向前
delay(100);
//检查左右距离并做出转向决定
if ( rightDistance < OK_DIST && leftDistance < OK_DIST) { // 如果左右方向距离均小于允许距离
// Serial.println("Turn back");
turnBack(); // 掉头
} else if ( rightDistance >= leftDistance) { // 如果右边距离大于左边距离
// Serial.println("Turn Right");
turnR90(); // 右转90度
} else { // 如果左边距离大于右边距离
// Serial.println("Turn Left");
turnL90(); // 左转90度
}
}
}
}
//检测距离
int look(uint8_t angle) {
tyler_2.setHeadPos(angle);
delay(500);
int Dist = readDistance();
// Serial.print(angle);
// Serial.print("度角距离 = "); Serial.println(Dist);
delay(100);
return Dist;
}
// 检查左右方向距离并返回专项方向
void autoTurn() {
// 检查右侧距离
tyler_2.setHeadPos(5);
delay(500);
int rightDist = readDistance();
delay(100);
tyler_2.setHeadPos(175);
delay(500);
int leftDist = readDistance();
delay(100);
//将头部调整到正前方
tyler_2.setHeadPos(90);
delay(500);
//检查左右距离并做出转向决定
if ( rightDist < OK_DIST && leftDist < OK_DIST) { // 如果左右方向距离均小于允许距离
turnBack(); // 掉头
return;
} else if ( rightDist >= leftDist) { // 如果右边距离大于左边距离
turnR90(); // 右转90度
return;
} else { // 如果左边距离大于右边距离
turnL90(); // 左转90度
return;
}
}
// 左转90度
void turnL90() {
tyler_2.turnL();
delay(TURN_LEFT_90);
}
// 右转90度
void turnR90() {
tyler_2.turnR();
delay(TURN_RIGHT_90);
}
// 掉头
void turnBack() {
tyler_2.turnL();
delay(TURN_BACK);
}
int readDistance() {
delay(50);
int cm = tyler_2.getDistance();
//Serial.println(cm);
if (cm == 0)
{
cm = MAX_DISTANCE;
}
return cm;
}
/*
控制指令字符
模式控制
AUTO --- 自动避障模式
MAN --- 人工控制模式
GEST --- 体感控制模式
运行控制
8 --- 前进
2 --- 后退
4 --- 左转
6 --- 右转
5 --- 停车
7 --- 左前
9 --- 右前
1 --- 左后
3 --- 右后
A --- 自动模式
M --- 手动模式
G --- 体感模式
*/
// 通过串口输出运行模式信息
void modeReport() {
if (cmdCharSave != cmdChar) {
cmdCharSave = cmdChar;
if (systemMode == MAN) {
Serial.println("tyler_2: MANUAL Mode");
opReport(); // 通过串口输出手动模式下的操作指令
} else if (systemMode == AUTO) {
Serial.println("tyler_2: AUTO Mode");
} else if (systemMode == GEST) {
Serial.println("tyler_2: GEST Mode");
opReport(); // 通过串口输出体感模式下的操作指令
}
}
}
// 通过串口输出当前操作信息
void opReport() {
switch (cmdChar) {
case '8':
Serial.println("tyler_2: FORWARD");
break;
case '2':
Serial.println("tyler_2: BACKWARD");
break;
case '4':
Serial.println("tyler_2: TURN LEFT");
break;
case '6':
Serial.println("tyler_2: TURN RIGHT");
break;
case '5':
Serial.println("tyler_2: STOP");
break;
case '7':
Serial.println("tyler_2: FORWARD LEFT");
break;
case '9':
Serial.println("tyler_2: FORWARD RIGHT");
break;
case '1':
Serial.println("tyler_2: BACKWARD LEFT");
break;
case '3':
Serial.println("tyler_2: BACKWARD RIGHT");
break;
}
}
tyler_2.h文件
此部分主要修改了auto模式部分的代码，并增加了look函数，
#ifndef TYLER_2_H_
#define TYLER_2_H_
#include "Arduino.h"
#include <AFMotor.h>
#include <NewPing.h>
// #include <Servo.h> //不调用此库，因为有雷达的原因，改为程序控制，防止舵机抖动。
#define DEFAULT_SPEED 180 //默认太乐1号行动速度，100~255，速度过快使雷达的反应变慢，容易撞墙
#define MAX_DISTANCE 200 //定义超声波信号发射的最大距离为200
// 太乐1号类
// class NewPing;
class Tyler_2{
public:
// 只提供车轮电机方向设置参数的构造函数
/* Tyler_2(bool dir1, bool dir2, bool dir3, bool dir4);
// 提供车轮电机方向设置参数，车轮电机速度参数的构造函数
Tyler_2(bool dir1, bool dir2, bool dir3, bool dir4, byte motorSpeed); */
// 提供车轮电机方向设置参数，车轮电机速度参数， 测距传感器引脚参数的构造函数
Tyler_2(bool dir1, bool dir2, bool dir3, bool dir4, byte motorSpeed, uint8_t trigPin, uint8_t echoPin,byte servoPin,unsigned int max_cm_distance=MAX_DISTANCE);
// 提供车轮电机方向设置参数，车轮电机速度参数， 测距传感器引脚参数以及舵机控制引脚参数的构造函数
// Tyler_2(bool dir1, bool dir2, bool dir3, bool dir4, byte motorSpeed, int trigPin, int echoPin, int servoPin);
void forward(); // 前进
void backward(); // 后退
void turnL(); // 左转
void turnR(); // 右转
void forwardL(); // 左前
void forwardR(); // 右前
void backwardL(); // 左后
void backwardR(); // 右后
void stop(); // 停车
int getDistance(); // 获取距离传感器读数
void setHeadPos(int pos); // 设定舵机角度
int getHeadPos(); // 获取舵机角度
// void headServoIni(); // 舵机初始化 acktomas修改
private:
void dcMotorIni(bool dir1, bool dir2, bool dir3, bool dir4); // 车轮电机初始化设置
AF_DCMotor* dcMotor1 = new AF_DCMotor(1); // 车轮电机1
AF_DCMotor* dcMotor2 = new AF_DCMotor(2); // 车轮电机2
AF_DCMotor* dcMotor3 = new AF_DCMotor(3); // 车轮电机3
AF_DCMotor* dcMotor4 = new AF_DCMotor(4); // 车轮电机4
// Servo headServo; // 建立头部舵机对象,acktomas修改
int headServoPin; // 舵机控制引脚
byte dcMotor1Forward; // 车轮电机1正转参数
byte dcMotor1Backward; // 车轮电机1反转参数
byte dcMotor2Forward; // 车轮电机2正转参数
byte dcMotor2Backward; // 车轮电机2反转参数
byte dcMotor3Forward; // 车轮电机3正转参数
byte dcMotor3Backward; // 车轮电机3反转参数
byte dcMotor4Forward; // 车轮电机4正转参数
byte dcMotor4Backward; // 车轮电机4反转参数
int hcTrig; // 超声测距传感器Trig引脚
int hcEcho; // 超声测距传感器Echo引脚
long duration, cm; // 超声测距传感器用变量
uint8_t lastServoAngle;
NewPing sonar;
};
#endif
tyler_2.cpp
#include "Tyler_2.h"
//#include <NewPing.h>
// 只提供车轮电机方向设置参数的构造函数
/* Tyler_2::Tyler_2(bool dir1, bool dir2, bool dir3, bool dir4):sonar(1, 1, 1){
dcMotor1->setSpeed(DEFAULT_SPEED);
dcMotor2->setSpeed(DEFAULT_SPEED);
dcMotor3->setSpeed(DEFAULT_SPEED);
dcMotor4->setSpeed(DEFAULT_SPEED);
dcMotorIni( dir1, dir2, dir3, dir4);
}
// 提供车轮电机方向设置参数，车轮电机速度参数的构造函数
Tyler_2::Tyler_2(bool dir1, bool dir2, bool dir3, bool dir4, byte motorSpeed):sonar(1, 1, 1){
dcMotor1->setSpeed(motorSpeed);
dcMotor2->setSpeed(motorSpeed);
dcMotor3->setSpeed(motorSpeed);
dcMotor4->setSpeed(motorSpeed);
dcMotorIni( dir1, dir2, dir3, dir4);
} */
// 提供车轮电机方向设置参数，车轮电机速度参数， 测距传感器引脚参数的构造函数
Tyler_2::Tyler_2(bool dir1, bool dir2, bool dir3, bool dir4, byte motorSpeed, uint8_t trigPin, uint8_t echoPin,byte servoPin,unsigned int max_cm_distance=MAX_DISTANCE):sonar(trigPin, echoPin, max_cm_distance){
headServoPin = servoPin;
hcTrig = trigPin;
hcEcho = echoPin;
dcMotor1->setSpeed(motorSpeed);
dcMotor2->setSpeed(motorSpeed);
dcMotor3->setSpeed(motorSpeed);
dcMotor4->setSpeed(motorSpeed);
dcMotorIni( dir1, dir2, dir3, dir4);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
}
// 提供车轮电机方向设置参数，车轮电机速度参数， 测距传感器引脚参数以及舵机控制引脚参数的构造函数
/* Tyler_2::Tyler_2(bool dir1, bool dir2, bool dir3, bool dir4, byte motorSpeed, int trigPin, int echoPin, int servoPin){
headServoPin = servoPin;
hcTrig = trigPin;
hcEcho = echoPin;
dcMotor1->setSpeed(motorSpeed);
dcMotor2->setSpeed(motorSpeed);
dcMotor3->setSpeed(motorSpeed);
dcMotor4->setSpeed(motorSpeed);
dcMotorIni( dir1, dir2, dir3, dir4);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
} */
// 设置舵机位置
void Tyler_2::setHeadPos(int angle){
for (int i = 0; i < 50; i++) {
int pulsewidth = (angle * 11) + 500; //将角度转化为500-2480的脉宽值
digitalWrite(headServoPin, HIGH); //将舵机接口电平至高
delayMicroseconds(pulsewidth); //延时脉宽值的微秒数
digitalWrite(headServoPin, LOW); //将舵机接口电平至低
delayMicroseconds(20000 - pulsewidth);
}
lastServoAngle = angle;
// Serial.println(angle);
}
// 获取舵机位置
int Tyler_2::getHeadPos(){
// return headServo.read();
return lastServoAngle;
}
// 读取传感器距离读数（单位为厘米）
int Tyler_2::getDistance(){
/* digitalWrite(hcTrig, LOW);
delayMicroseconds(5);
digitalWrite(hcTrig, HIGH);
delayMicroseconds(10);
digitalWrite(hcTrig, LOW);
duration = pulseIn(hcEcho, HIGH);
cm = (duration/2) / 29.1; */
delay(50);
int cm = sonar.ping_cm();
if (cm == 0)
{
cm = MAX_DISTANCE;
}
return cm;
}
// 车轮电机初始化(设置各个车轮转动方向，使其符合程序控制需要)
void Tyler_2::dcMotorIni(bool dir1, bool dir2, bool dir3, bool dir4){
if (dir1 == 1){
dcMotor1Forward = FORWARD;
dcMotor1Backward = BACKWARD;
} else {
dcMotor1Forward = BACKWARD;
dcMotor1Backward = FORWARD;
}
if (dir2 == 1){
dcMotor2Forward = FORWARD;
dcMotor2Backward = BACKWARD;
} else {
dcMotor2Forward = BACKWARD;
dcMotor2Backward = FORWARD;
}
if (dir3 == 1){
dcMotor3Forward = FORWARD;
dcMotor3Backward = BACKWARD;
} else {
dcMotor3Forward = BACKWARD;
dcMotor3Backward = FORWARD;
}
if (dir4 == 1){
dcMotor4Forward = FORWARD;
dcMotor4Backward = BACKWARD;
} else {
dcMotor4Forward = BACKWARD;
dcMotor4Backward = FORWARD;
}
}
// 前进
void Tyler_2::forward(){
dcMotor1->run(dcMotor1Forward);
dcMotor2->run(dcMotor2Forward);
dcMotor3->run(dcMotor3Forward);
dcMotor4->run(dcMotor4Forward);
}
// 后退
void Tyler_2::backward(){
dcMotor1->run(dcMotor1Backward);
dcMotor2->run(dcMotor2Backward);
dcMotor3->run(dcMotor3Backward);
dcMotor4->run(dcMotor4Backward);
}
// 左转
void Tyler_2::turnL(){
dcMotor1->run(dcMotor1Forward);
dcMotor2->run(dcMotor2Forward);
dcMotor3->run(dcMotor3Backward);
dcMotor4->run(dcMotor4Backward);
}
// 右转
void Tyler_2::turnR(){
dcMotor1->run(dcMotor1Backward);
dcMotor2->run(dcMotor2Backward);
dcMotor3->run(dcMotor3Forward);
dcMotor4->run(dcMotor4Forward);
}
// 左前
void Tyler_2::forwardL(){
dcMotor1->run(dcMotor1Forward);
dcMotor2->run(dcMotor2Forward);
dcMotor3->run(RELEASE);
dcMotor4->run(RELEASE);
}
// 右前
void Tyler_2::forwardR(){
dcMotor1->run(RELEASE);
dcMotor2->run(RELEASE);
dcMotor3->run(dcMotor3Forward);
dcMotor4->run(dcMotor4Forward);
}
// 左后
void Tyler_2::backwardL(){
dcMotor1->run(RELEASE);
dcMotor2->run(RELEASE);
dcMotor3->run(dcMotor3Backward);
dcMotor4->run(dcMotor4Backward);
}
// 右后
void Tyler_2::backwardR(){
dcMotor1->run(dcMotor1Backward);
dcMotor2->run(dcMotor2Backward);
dcMotor3->run(RELEASE);
dcMotor4->run(RELEASE);
}
// 停止
void Tyler_2::stop(){
dcMotor1->run(RELEASE);
dcMotor2->run(RELEASE);
dcMotor3->run(RELEASE);
dcMotor4->run(RELEASE);
}
扫一扫
1114
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
