零知开源——基于STM32F103RBT6和ADXL335实现SG90舵机姿态控制系统

于 2025-08-25 16:04:02 发布
阅读量888 收藏 15
点赞数 9
CC 4.0 BY-SA版权
文章标签： stm32 单片机 开源
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/lingzhilab/article/details/150771660

 ✔零知IDE 是一个真正属于国人自己的开源软件平台，在开发效率上超越了Arduino平台并且更加容易上手，大大降低了开发难度。零知开源在软件方面提供了完整的学习教程和丰富示例代码，让不懂程序的工程师也能非常轻而易举的搭建电路来创作产品，测试产品。快来动手试试吧！

✔访问零知实验室，获取更多实战项目和教程资源吧！

www.lingzhilab.com

目录

一、硬件设计部分

1.1 硬件清单

1.2 接线方案

1.3 具体接线图

1.4 接线实物图

二、软件设计部分

2.1 程序结构依赖

2.2 系统初始化

2.3 主函数循环

2.4 ADXL335自动校准

2.5 控制舵机转动

2.6 项目完整代码

三、实现结果展示

3.1 自动校准界面

3.2 传感器操作

3.3 视频演示

四、ADXL335运行过程

4.1 加速度计核心原理

4.2 加速度计数据处理

4.3 加速度计校准

五、常见问题解答

Q1: 校准后数据仍然不准确怎么办？

Q2: 为什么我的传感器读数不稳定？

Q3: 如何提高系统响应速度？

（1）项目概述

        本项目是一个基于ADXL335三轴加速度计和SG90舵机的智能姿态监测与控制系统。通过精确测量物体的倾斜角度，系统能够实时显示三轴加速度数据，并根据预设的映射关系控制舵机转动。项目采用了ST7789 TFT显示屏提供直观的视觉反馈，实现了从传感器数据采集、处理到执行器控制的完整闭环系统。

（2）项目亮点

        >采用多采样平均和数字滤波技术，显著提高传感器数据精度
        >内置自动校准算法，自动计算各轴的偏移量和比例因子
        >支持X轴和Y轴两种控制模式，适应不同应用场景

（3）项目难点及解决方案

        问题描述：ADXL335输出的模拟信号存在明显噪声，导致角度计算不稳定

解决方案：

        采用多采样求平均技术，减少随机误差。实现数字低通滤波算法，平滑数据输出。使用中位数滤波去除异常值

一、硬件设计部分

1.1 硬件清单

组件名称规格型号数量备注
零知标准板STM32F103RBT6主控1主控制器
ADXL335模块三轴加速度计1姿态传感器
ST7789 TFT显示屏1.3寸 IPS1显示模块
舵机SG901执行器

1.2 接线方案

零知标准板(STM32F103RBT6)ST7789(SPI)ADXL335舵机(SG90)
3.3V / 5VVCCVCCVCC红线
GNDGNDGNDGND棕线
6CS//
7SCL//
8SDA//
4RES//
2DC//
X-OUT/A1/
Y-OUT/A2/
Z-OUT/A3/
9//PWM黄线

1.3 具体接线图

​

1.4 接线实物图

        零知标准板的GND、3.3V和9数字引脚分别连接到舵机的红线、棕线及黄线

​

二、软件设计部分

2.1 程序结构依赖

        本项目代码采用模块化设计，主要包含以下功能模块：传感器数据采集与处理、校准算法实现、姿态角度计算、舵机控制逻辑、显示界面管理、用户交互处理

2.2 系统初始化

#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_ST7789.h>
#include <Servo.h>

// ST7789 显示屏引脚定义
#define TFT_CS   6
#define TFT_RST  4
#define TFT_DC   2
#define TFT_MOSI 8
#define TFT_SCLK 7

// 初始化ST7789显示屏
Adafruit_ST7789 tft = Adafruit_ST7789(TFT_CS, TFT_DC, TFT_MOSI, TFT_SCLK, TFT_RST);

// 定义显示屏参数
#define SCREEN_WIDTH  240
#define SCREEN_HEIGHT 240

// 引脚定义
const int xPin = A1;
const int yPin = A2;
const int zPin = A3;
const int servoPin = 9;

Servo myServo;

// 校准参数
float x_offset = 0;
float y_offset = 0;
float z_offset = 0;
float x_scale = 1.0;
float y_scale = 1.0;
float z_scale = 1.0;

// 姿态角
float pitch = 0;
float roll = 0;

// 系统状态
bool calibrated = false;
bool useXAxis = false; // true: 使用X轴控制舵机, false: 使用Y轴控制舵机

// 显示相关变量
unsigned long lastDisplayUpdate = 0;
const int DISPLAY_UPDATE_INTERVAL = 200; // 显示更新间隔(ms)

void setup() {
  // 初始化TFT显示屏
  tft.init(SCREEN_WIDTH, SCREEN_HEIGHT);
  tft.setRotation(1);
  tft.fillScreen(ST77XX_BLACK);
  tft.setTextColor(ST77XX_WHITE);
  
  // 显示启动画面
  tft.setTextSize(3);
  tft.setCursor(40, 80);
  tft.print("ADXL335");
  tft.setTextSize(2);
  tft.setCursor(60, 130);
  tft.print("Loading...");
  delay(1500);
  
  // 初始化舵机
  myServo.attach(servoPin);
  myServo.write(90); // 初始位置
  
  // 预热传感器
  delay(1000);
  
  // 进入校准流程
  displayCalibrationScreen();
  calibrateSensor();
  
  // 校准完成，进入主界面
  tft.fillScreen(ST77XX_BLACK);
  drawStaticUI();
}

        定义初始值并完成硬件初始化和系统启动的流程

2.3 主函数循环

void loop() {
  // 读取传感器数据
  int x_raw = analogRead(xPin);
  int y_raw = analogRead(yPin);
  int z_raw = analogRead(zPin);
  
  // 应用校准
  float x_accel = (x_raw - x_offset) * x_scale;
  float y_accel = (y_raw - y_offset) * y_scale;
  float z_accel = (z_raw - z_offset) * z_scale;
  
  // 计算姿态角
  calculateAngles(x_accel, y_accel, z_accel);
  
  // 控制舵机
  controlServo();
  
  // 更新显示（限制刷新频率）
  if (millis() - lastDisplayUpdate > DISPLAY_UPDATE_INTERVAL) {
    updateDisplayValues(x_accel, y_accel, z_accel);
    lastDisplayUpdate = millis();
  }
  
  delay(200); // 20Hz更新率
}

        负责持续读取传感器数据、处理数据、控制舵机和更新显示

        DISPLAY_UPDATE_INTERVAL = 200参数：显示更新间隔（毫秒），控制显示刷新频率

2.4 ADXL335自动校准

void calibrateSensor() {
  long x_sum = 0, y_sum = 0, z_sum = 0;
  const int samples = 500;
  
  for (int i = 0; i < samples; i++) {
    x_sum += analogRead(xPin);
    y_sum += analogRead(yPin);
    z_sum += analogRead(zPin);
    
    // 显示进度条
    int progress = map(i, 0, samples, 0, 200);
    tft.drawRect(20, 150, 200, 20, ST77XX_WHITE);
    tft.fillRect(20, 150, progress, 20, ST77XX_GREEN);
    
    delay(5);
  }
  
  // 计算偏移量和比例因子
  x_offset = x_sum / samples;
  y_offset = y_sum / samples;
  z_offset = z_sum / samples - 512;
  
  x_scale = 1.0 / 102.4;
  y_scale = 1.0 / 102.4;
  z_scale = 1.0 / 102.4;
}

        samples = 500：校准采样次数，影响校准精度
        512：1.65V对应的ADC值(3.3V参考电压)
        102.4：理论灵敏度值(330mV/g对应的ADC值)

2.5 控制舵机转动

void controlServo() {
  // 根据选择的轴控制舵机
  float controlValue = useXAxis ? (x_offset - analogRead(xPin)) * x_scale : 
                                 (y_offset - analogRead(yPin)) * y_scale;
  
  // 将加速度值映射到舵机角度
  int servoAngle;
  if (controlValue >= 0) {
    servoAngle = map(controlValue, 0, 3.5, 90, 0);
  } else {
    servoAngle = map(controlValue, -3.5, 0, 180, 90);
  }
  
  servoAngle = constrain(servoAngle, 0, 180);
  myServo.write(servoAngle);
}

        根据加速度值控制舵机转动

2.6 项目完整代码 

#include <SPI.h>
#include <Adafruit_GFX.h>
#include <Adafruit_ST7789.h>
#include <Servo.h>

// ST7789 显示屏引脚定义
#define TFT_CS   6
#define TFT_RST  4
#define TFT_DC   2
#define TFT_MOSI 8
#define TFT_SCLK 7

// 初始化ST7789显示屏
Adafruit_ST7789 tft = Adafruit_ST7789(TFT_CS, TFT_DC, TFT_MOSI, TFT_SCLK, TFT_RST);

// 定义显示屏参数
#define SCREEN_WIDTH  240
#define SCREEN_HEIGHT 240

// 引脚定义
const int xPin = A1;
const int yPin = A2;
const int zPin = A3;
const int servoPin = 9;

Servo myServo;

// 校准参数
float x_offset = 0;
float y_offset = 0;
float z_offset = 0;
float x_scale = 1.0;
float y_scale = 1.0;
float z_scale = 1.0;

// 姿态角
float pitch = 0;
float roll = 0;

// 系统状态
bool calibrated = false;
bool useXAxis = false; // true: 使用X轴控制舵机, false: 使用Y轴控制舵机

// 显示相关变量
unsigned long lastDisplayUpdate = 0;
const int DISPLAY_UPDATE_INTERVAL = 200; // 显示更新间隔(ms)

void setup() {
  // 初始化TFT显示屏
  tft.init(SCREEN_WIDTH, SCREEN_HEIGHT);
  tft.setRotation(1);
  tft.fillScreen(ST77XX_BLACK);
  tft.setTextColor(ST77XX_WHITE);
  
  // 显示启动画面
  tft.setTextSize(3);
  tft.setCursor(40, 80);
  tft.print("ADXL335");
  tft.setTextSize(2);
  tft.setCursor(60, 130);
  tft.print("Loading...");
  delay(1500);
  
  // 初始化舵机
  myServo.attach(servoPin);
  myServo.write(90); // 初始位置
  
  // 预热传感器
  delay(1000);
  
  // 进入校准流程
  displayCalibrationScreen();
  calibrateSensor();
  
  // 校准完成，进入主界面
  tft.fillScreen(ST77XX_BLACK);
  drawStaticUI();
}

void loop() {
  // 读取传感器数据
  int x_raw = analogRead(xPin);
  int y_raw = analogRead(yPin);
  int z_raw = analogRead(zPin);
  
  // 应用校准
  float x_accel = (x_raw - x_offset) * x_scale;
  float y_accel = (y_raw - y_offset) * y_scale;
  float z_accel = (z_raw - z_offset) * z_scale;
  
  // 计算姿态角
  calculateAngles(x_accel, y_accel, z_accel);
  
  // 控制舵机
  controlServo();
  
  // 更新显示（限制刷新频率）
  if (millis() - lastDisplayUpdate > DISPLAY_UPDATE_INTERVAL) {
    updateDisplayValues(x_accel, y_accel, z_accel);
    lastDisplayUpdate = millis();
  }
  
  delay(200); // 20Hz更新率
}

// 显示校准界面
void displayCalibrationScreen() {
  tft.fillScreen(ST77XX_BLACK);
  tft.setTextSize(2);
  tft.setTextColor(ST77XX_YELLOW);
  tft.setCursor(10, 40);
  tft.print("ADXL335 Calibration");
  
  tft.setTextSize(2);
  tft.setTextColor(ST77XX_WHITE);
  tft.setCursor(50, 90);
  tft.print("Place sensor");
  tft.setCursor(70, 120);
  tft.print("flat");
  
  tft.setTextSize(1);
  tft.setCursor(40, 170);
  tft.print("Calibrating...");
}

// 绘制静态UI元素
void drawStaticUI() {
  // 绘制标题
  tft.setTextSize(2);
  tft.setTextColor(ST77XX_CYAN);
  tft.setCursor(60, 5);
  tft.print("ADXL335");
  
  // 绘制轴标签
  tft.setTextSize(1);
  tft.setTextColor(ST77XX_GREEN);
  tft.setCursor(20, 35);
  tft.print("Pitch:");
  tft.setCursor(20, 55);
  tft.print("Roll:");
  
  tft.setCursor(130, 35);
  tft.print("X:");
  tft.setCursor(130, 55);
  tft.print("Y:");
  tft.setCursor(130, 75);
  tft.print("Z:");
  
  // 绘制舵机标签
  tft.setCursor(20, 95);
  tft.print("Servo:");
  
  // 绘制模式指示
  tft.setCursor(20, 115);
  tft.print("Mode:");
  tft.setCursor(70, 115);
  tft.print(useXAxis ? "X-Axis" : "Y-Axis");
  
  // 绘制角度指示条框架
  tft.drawRect(20, 135, 200, 20, ST77XX_WHITE);
  tft.setCursor(20, 160);
  tft.print("-90");
  tft.setCursor(110, 160);
  tft.print("0");
  tft.setCursor(200, 160);
  tft.print("90");
  
  // 绘制底部信息
  tft.setTextSize(1);
  tft.setTextColor(0x1D21);
  tft.setCursor(40, 200);
  tft.print("Press reset to recalibrate");
}

// 更新显示数值（局部刷新）
void updateDisplayValues(float x, float y, float z) {
  // 清除旧数值区域
  tft.fillRect(70, 35, 50, 15, ST77XX_BLACK); // Pitch
  tft.fillRect(70, 55, 50, 15, ST77XX_BLACK); // Roll
  
  tft.fillRect(150, 35, 70, 15, ST77XX_BLACK); // X
  tft.fillRect(150, 55, 70, 15, ST77XX_BLACK); // Y
  tft.fillRect(150, 75, 70, 15, ST77XX_BLACK); // Z
  
  tft.fillRect(70, 95, 40, 15, ST77XX_BLACK); // Servo
  
  // 更新角度值
  tft.setTextSize(1);
  tft.setTextColor(ST77XX_YELLOW);
  tft.setCursor(70, 35);
  tft.print(pitch, 1);
  tft.setCursor(70, 55);
  tft.print(roll, 1);
  
  // 更新加速度值
  tft.setCursor(150, 35);
  tft.print(x, 2);
  tft.setCursor(150, 55);
  tft.print(y, 2);
  tft.setCursor(150, 75);
  tft.print(z, 2);
  
  // 更新舵机角度
  tft.setCursor(70, 95);
  tft.print(myServo.read());
  
  // 更新角度指示条
  static int lastPitchBarPos = 110;
  int pitchBarPos = map(pitch, -90, 90, 20, 220);
  
  // 清除旧指示条位置
  if (lastPitchBarPos != pitchBarPos) {
    tft.fillRect(lastPitchBarPos, 137, 16, 16, ST77XX_BLACK);
    lastPitchBarPos = pitchBarPos;
  }
  
  // 绘制新指示条位置
  tft.fillRect(pitchBarPos, 137, 16, 16, ST77XX_RED);
}

// 传感器校准函数
void calibrateSensor() {
  tft.fillScreen(ST77XX_BLACK);
  tft.setTextSize(2);
  tft.setTextColor(ST77XX_YELLOW);
  tft.setCursor(50, 80);
  tft.print("Calibrating...");
  tft.setTextSize(1);
  tft.setCursor(60, 120);
  tft.print("Keep sensor flat");
  
  long x_sum = 0, y_sum = 0, z_sum = 0;
  const int samples = 500;
  
  // 采集多组数据求平均值
  for (int i = 0; i < samples; i++) {
    x_sum += analogRead(xPin);
    y_sum += analogRead(yPin);
    z_sum += analogRead(zPin);
    
    // 显示进度条
    int progress = map(i, 0, samples, 0, 200);
    tft.drawRect(20, 150, 200, 20, ST77XX_WHITE);
    tft.fillRect(20, 150, progress, 20, ST77XX_GREEN);
    
    delay(5);
  }
  
  // 计算偏移量（假设水平放置时Z轴应为1g，X和Y轴应为0g）
  x_offset = x_sum / samples;
  y_offset = y_sum / samples;
  z_offset = z_sum / samples - 512; // 512是1.65V对应的ADC值(3.3V参考电压)
  
  // 计算比例因子（使用理论灵敏度330mV/g）
  // ADC每g的值 = (330mV/3300mV) * 1024 = 102.4
  x_scale = 1.0 / 102.4;
  y_scale = 1.0 / 102.4;
  z_scale = 1.0 / 102.4;
  
  calibrated = true;
  
  // 显示校准完成
  tft.fillScreen(ST77XX_BLACK);
  tft.setTextSize(2);
  tft.setTextColor(ST77XX_GREEN);
  tft.setCursor(70, 100);
  tft.print("Done!");
  delay(1000);
}

// 计算俯仰和横滚角
void calculateAngles(float x, float y, float z) {
  // 转换为g值
  x = x * 0.10197; // 转换为g
  y = y * 0.10197;
  z = z * 0.10197;
  
  // 计算角度（弧度）
  pitch = atan2(x, sqrt(y*y + z*z));
  roll = atan2(y, sqrt(x*x + z*z));
  
  // 转换为角度
  pitch = pitch * 180 / PI;
  roll = roll * 180 / PI;
}

// 控制舵机
void controlServo() {
  // 根据选择的轴控制舵机
  float controlValue = useXAxis ? (x_offset - analogRead(xPin)) * x_scale : 
                                 (y_offset - analogRead(yPin)) * y_scale;
  
  // 将加速度值映射到舵机角度
  // +3.5到0映射90°到0°、0到-3.5映射0°到-90°
  int servoAngle;
  if (controlValue >= 0) {
    // +3.5到0映射90°到0°
    servoAngle = map(controlValue, 0, 3.5, 90, 0);
  } else {
    // 0到-3.5映射0°到-90°（但舵机角度不能为负，所以映射到0-90）
    servoAngle = map(controlValue, -3.5, 0, 180, 90);
  }
  
  // 限制舵机角度在0-180范围内
  servoAngle = constrain(servoAngle, 0, 180);
  myServo.write(servoAngle);
}

        x_offset, y_offset, z_offset：各轴的零g偏移量
        x_scale, y_scale, z_scale：各轴的比例因子，用于将ADC值转换为g值
        useXAxis：布尔值，选择使用X轴或Y轴控制舵机

三、实现结果展示

3.1 自动校准界面

(1)系统启动后显示启动画面，进入校准界面，按提示将传感器水平放置

​

(2)等待校准进度条完成(约30秒)，校准完成后自动进入主界面

3.2 传感器操作

        >观察主界面显示的三轴加速度值和角度值
        >倾斜传感器，观察数值变化和舵机响应

        ps:可通过修改代码中的useXAxis变量切换控制轴

3.3 视频演示

ADXL335角度传感器倾斜控制舵机角度

系统启动后将传感器放置水平状态进入自动校准模式，倾斜ADXL335观察舵机是否按预期运动

效果验证：

切换到选择的X轴和Y轴

        往轴正方向：观察舵机角度在0-90°之间变化

        往轴负方向：观察舵机角度在90-180°之间变化

四、ADXL335运行过程

4.1 加速度计核心原理

        ADXL335是基于MEMS技术的三轴加速度计，其核心原理是通过测量质量块在加速度作用下的位移来检测加速度。每个轴上都有一个可变电容，当加速度作用于传感器时，质量块移动导致电容变化，进而产生与加速度成正比的电压信号。

​

4.2 加速度计数据处理

(1)原始数据读取

        int raw_value = analogRead(pin);

        读取ADC值，范围0-1023（10位ADC），对应0-3.3V电压

(2)电压转换

        电压值 = (ADC值 / 1023) × 参考电压

(3)加速度值计算

        加速度(g) = (电压值 - 零g电压) / 灵敏度

        零g电压：1.65V（3.3V供电时） |  灵敏度：330mV/g

(4)角度计算

        俯仰角(pitch) = atan2(X, sqrt(Y² + Z²))    |   横滚角(roll) = atan2(Y, sqrt(X² + Z²))

        使用三角函数将加速度分量转换为倾斜角度

​

4.3 加速度计校准

由于制造公差和环境因素，每个传感器都需要校准：

偏移校准：消除零g误差

        偏移量 = 实际零g读数 - 理论零g读数(512)<

比例校准：修正灵敏度误差

        比例因子 = 理论灵敏度 / 实际灵敏度<

五、常见问题解答

Q1: 校准后数据仍然不准确怎么办？

A ：尝试以下方法：

        增加校准采样次数（500以上）
        确保校准时传感器完全静止
        尝试手动微调偏移量和比例因子

Q2: 为什么我的传感器读数不稳定？

A: 可能原因及解决方案：

        使用稳定的电源，添加滤波电容
        减少环境振动或添加减震措施
        检查连接是否牢固，线长是否合适

Q3: 如何提高系统响应速度？

A: 优化策略：

        减少采样数量
        优化代码结构，减少不必要的计算
        提高主循环执行频率

项目资源：

        加速度计数据手册：ADXL335 (Rev. B)

通过本项目成功实现了一个基于ADXL335加速度计的完整姿态监测与控制系统。点击获取更丰富的开源教程：

        零知实验室：https://www.lingzhilab.com/

lingzhilab
关注
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值