太极创客的项目太乐1号的改造_arduino舵机抖动问题-CSDN博客

本文链接：https://blog.csdn.net/acktomas/article/details/115621706

太极创客的项目太乐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);   
}