太极创客的项目太乐1号原始地址：http://www.taichi-maker.com/homepage/arduino-tutorial-index/arduino-hardware/#taile1
本文修改后的源代码下载地址：https://download.csdn.net/download/acktomas/
关于修改过程中类的引用的文章可参考：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); }