stm32笔记[9]-串口控制云台
摘要
基于stm32的云台控制程序,使用串口接收云台移动指令对云台进行控制.
使用软件实现的PWM波发生方式.
平台信息
- Arduino IDE
- stm32f103c8t6
接口
- S3:servo_bottom_pin:PA2
- S4:servo_top_pin:PA3
- S5:laser_pin:PB4
- S21:sl_in_pin:PA8
- S22:sr_out_pin:PB14
- S23:sl_out_pin:PB15
- S24:sr_out_pin:PA11
- buzzer_pin:PA12
- uart2_tx:PA2
- uart2_rx:PA3
舵机控制简介
数字型和模拟型舵机
[https://blog.csdn.net/u012308586/article/details/107974117]
传统模拟舵机和数字比例舵机（或称之为标准舵机）的电子电路中无MCU微控制器，一般都称之为模拟舵机。老式模拟舵机由功率运算放大器等接成惠斯登电桥，根据接收到模拟电压控制指令和机械连动位置传感器（电位器）反馈电压之间比较产生的差分电压，驱动有刷直流电机伺服电机正/反运转到指定位置。数字比例舵机是模拟舵机最好的类型，由直流伺服电机、直流伺服电机控制器集成电路(IC)，减速齿轮组和反馈电位器组成，它由直流伺服电机控制芯片直接接收PWM（脉冲方波，一般周期为20ms，脉宽1~2 ms，脉宽1 ms为上限位置，1.5ms为中位,2ms为下限位置）形式的控制驱动信号，迅速驱动电机执行位置输出,直至直流伺服电机控制芯片检测到位置输出连动电位器送来的反馈电压与PWM控制驱动信号的平均有效电压相等,停止电机，完成位置输出。
舵机的伺服系统由可变宽度的脉冲来进行控制，控制线是用来传送脉冲的。脉冲的参数有最小值，最大值，和频率。一般而言，舵机的基准信号都是周期为20ms，宽度为1.5ms。这个基准信号定义的位置为中间位置。舵机有最大转动角度，中间位置的定义就是从这个位置到最大角度与最小角度的量完全一样。最重要的一点是，不同舵机的最大转动角度可能不相同，但是其中间位置的脉冲宽度是一定的，那就是1.5ms。
PWM控制舵机
[https://www.cnblogs.com/zhoubatuo/p/6138033.html]
当控制系统发出指令，让舵机移动到某一位置，并让他保持这个角度，这时外力的影响不会让他角度产生变化，但是这个是由上限的，上限就是他的最大扭力。除非控制系统不停的发出脉冲稳定舵机的角度，舵机的角度不会一直不变。
当舵机接收到一个小于1.5ms的脉冲，输出轴会以中间位置为标准，逆时针旋转一定角度。接收到的脉冲大于1.5ms情况相反。
占空比与旋转角度如图(180˚舵机):
舵机死区
舵机的死区是指在舵机的中立位置附近，输入信号无法对舵机产生实际的动作或超过该范围的输入信号也无法使舵机产生更大的动作5。舵机的死区时间是指舵机在中立位置附近停止运动的时间间隔。常见小型舵机的死区时间约为5微秒，对应的角度约为0.45度。也就是说，如果想将舵机旋转到某个角度，实际上舵机的停止位置会在目标角度的附近，具体范围取决于死区时间。较小的死区时间可以提高舵机的控制精度，但如果齿轮结构没有那么高的精度，可能会导致舵机频繁修正，从而影响舵机的寿命。
舵机扫尾现象
我们提出了 1 个扫尾的新概念:当 CPU 执行完 8 个位的下降沿操作后(最多为 2.5ms)， 会有向下 1 个周期过渡的时间间隔，其主要为 2 个功能:
1保证下降沿的准确性;
2为舵机的跟踪留出足够的时间;
当 PWM 信号以最小变化量即(1DIV=8us)依次变化时，舵机的分辨率最高，但是速度会减 慢。
例如:先发一个 PWM 信号 N=125，相隔 20ms 后再发 1 个 PWM 信号 N=126。那么舵机在 20ms 内转动了 0.74 度，计算得出:ω=0.74 度/20ms = 37 度/秒;
HG14-M 舵机空载时:ω=300 度/秒 发现与最快速度相差 8 倍之多!
舵机从 PWM_A 发出后开始 转动，经过△T 时间后接收完毕 PWM_B 信号后，又重新开始新的转动。
实现
命令格式
- 云台底部电机旋转到指定角度,顶部电机不动(361˚就是对应的舵机不动):
<angle 50 361;> - 云台底部电机不动,顶部电机旋转到指定角度:
<angle 361 50;> - 云台旋转到指定角度:
<angle 40 50;>
添加自定义串口命令
示例:
//判断token[0]=='angle_delta'
//直接移动指定角度(相对)
if(strcmp(tokens[0],"angle_delta") == 0){
int delta_angle_polar,delta_angle_z;
// 检查是否有足够的标记以便进行转换
if (count >= 3) {
delta_angle_polar = atoi(tokens[1]);
delta_angle_z = atoi(tokens[2]);
#ifdef DEBUG
Serial.printf("CMD:");
Serial.printf("%s(%d,%d)",tokens[0],delta_angle_polar,delta_angle_z);
Serial.printf("\n");
#endif
}
}
tokens[0]为命令起始,count为命令的参数数量;
代码
/*
功能:
- 串口控制二维云台
算法:
- 坐标映射
总体流程:
串口接收目标位置->移动舵机到位置
依赖库:
- Adafruit PWM(其实没用到)
- Simpletimer(其实没用到)
接口:
- S3:servo_bottom_pin:PA2
- S4:servo_top_pin:PA3
- S5:laser_pin:PB4
- S21:sl_in_pin:PA8
- S22:sr_out_pin:PB14
- S23:sl_out_pin:PB15
- S24:sr_out_pin:PA11
- buzzer_pin:PA12
- uart2_tx:PA2
- uart2_rx:PA3
*/
#include <Arduino.h>
#include <Servo.h>
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <Adafruit_PWMServoDriver.h>
#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <HardwareSerial.h>
#include <SoftwareSerial.h>
#include <Simpletimer.h>
#define DEBUG 1
#define SERVOMIN 150 // This is the 'minimum' pulse length count (out of 4096)
#define SERVOMAX 600 // This is the 'maximum' pulse length count (out of 4096)
#define USMIN 600 // This is the rounded 'minimum' microsecond length based on the minimum pulse of 150
#define USMAX 2400 // This is the rounded 'maximum' microsecond length based on the maximum pulse of 600
#define SERVO_FREQ 50 // Analog servos run at ~50 Hz updates
#define MySerial Serial2
#define SERVO_BOTTOM 1
#define SERVO_TOP 2
//#define MPU6050_SCL PB10//s11
//#define MPU6050_SDA PB11//s12
const uint8_t servo_bottom_pin = PA0;//s1,更改为20kg舵机
const uint8_t servo_top_pin = PA1;//s2
const uint8_t buzzer_pin = PA12;
const int mpu6050_scl_pin = PB10;
const int mpu6050_sda_pin = PB11;
const int laser_pin = PB4;//S5
const int sl_in_pin = PA8;//S21
const int sr_in_pin = PB14;//S22
const int uart3_rx_pin = PC_10;//(uart2)
const int uart3_tx_pin = PC_11;//(uart2)
const int sl_out_pin = PB15;//S23,继电器
const int sr_out_pin = PA11;//S24,继电器,pc9
const int key_pin = PB12;//随机运动按键
// 定义ServoClass类
class ServoClass {
private:
int pin; // 舵机接口引脚
int minPulseWidth; // 最小脉宽值（对应最小角度）
int maxPulseWidth; // 最大脉宽值（对应最大角度）
public:
ServoClass(int p) {
pin = p;
minPulseWidth = 500; // 最小脉宽值为500微秒=0.5ms
maxPulseWidth = 2500; // 最大脉宽值为2500微秒=2.5ms
}
void attach(int p, int min, int max) {
pin = p;
minPulseWidth = min;
maxPulseWidth = max;
}
void writeMicroseconds(int pulsewidth) {
//int move_step = 30; //步长
for(int i=0;i<30;i++){
if (pulsewidth < minPulseWidth) {
pulsewidth = minPulseWidth;
} else if (pulsewidth > maxPulseWidth) {
pulsewidth = maxPulseWidth;
}
pinMode(pin,OUTPUT);
digitalWrite(pin, HIGH); // 将舵机接口电平置高
delayMicroseconds(pulsewidth); // 延时脉宽值的微秒数
digitalWrite(pin, LOW); // 将舵机接口电平置低
//delayMicroseconds(maxPulseWidth - pulsewidth + 1); // 延时周期内剩余时间
delayMicroseconds(20000 - pulsewidth); // 延时周期内剩余时间
}
}
};
//串口接收相关
#define BUFFER_SIZE 39
uint16_t usart_rx_buffer_pos=0;
uint8_t usart_rx_buffer[BUFFER_SIZE];
uint8_t usart_cmds[BUFFER_SIZE];
HardwareSerial MySerial(PA3,PA2);//rx,tx
//SoftwareSerial MySerial(uart3_rx_pin,uart3_tx_pin);//rx,tx
ServoClass servo_bottom(servo_bottom_pin); // 底部舵机
ServoClass servo_top(servo_top_pin); // 顶部舵机
// Adafruit_PWMServoDriver servo_bottom_pwm = Adafruit_PWMServoDriver();// 底部舵机
// Adafruit_PWMServoDriver servo_top_pwm = Adafruit_PWMServoDriver();// 顶部舵机
Adafruit_MPU6050 mpu6050;
TwoWire mpu6050_wire(mpu6050_scl_pin,mpu6050_sda_pin); // 创建一个 TwoWire 对象并指定 SCL 和 SDA 引脚
void process_usart_buffer(void);//解析串口命令
void servo_cmd_decode_xyz2polar(uint8_t * cmd);//解析串口命令
void servo_polar(int angle_polar,int angle_z);//移动云台(角度)
void servo_check(void);//舵机云台开机初始化校准
void buzzer_buzz(void);//蜂鸣器短鸣
void buzzer_fast(void);//蜂鸣器超短鸣
void test_servo_line(void);//斜线测试
void mpu6050_test(void);//test mpu6050
float servo_map(float value, float fromLow, float fromHigh, float toLow, float toHigh);//映射
void laser(void);//点亮激光
void mouse_send(char * key);//鼠标发送左/右信号
void servo_angle(int angle,int angle_number);//根据角度移动指定舵机
//画面尺寸
const int screen_x=320;
const int screen_y=240;
//参数
float delta_x_coefficient = 0.2;//x轴缩放因子
float delta_y_coefficient = 0.2;//y轴缩放因子
//状态变量
int uart_ack=0;//与串口设备通信ok
int angle_polar_pos = 0;//底部舵机初始角度
int angle_z_pos = 0;//顶部舵机初始角度
/*
*@功能:初始化
**/
void setup() {
//初始化串口
Serial.begin(115200);
MySerial.begin(115200);
#ifdef DEBUG
buzzer_buzz();
delay(500);
buzzer_buzz();
#endif
pinMode(servo_bottom_pin,OUTPUT);
pinMode(servo_top_pin,OUTPUT);
//laser init
pinMode(laser_pin,OUTPUT);
digitalWrite(laser_pin,HIGH);
//init keys
// 初始化按钮引脚
pinMode(sl_in_pin, INPUT_PULLUP); // 启用上拉电阻以保持按钮在未按下时为高电平
pinMode(sr_in_pin, INPUT_PULLUP);
pinMode(sl_out_pin, OUTPUT); // 启用开漏以保持按钮在未按下时为高电平
pinMode(sr_out_pin, OUTPUT);
digitalWrite(sl_out_pin,LOW);
digitalWrite(sr_out_pin,LOW);
//初始化随机按钮
pinMode(key_pin,INPUT_PULLUP);// 启用上拉电阻以保持按钮在未按下时为高电平
//绑定云台,设置时间范围(500,2500)
servo_bottom.attach(servo_bottom_pin,500,2500);
servo_top.attach(servo_top_pin,500,2500);
//初始化校准云台
//servo_check();
}
void loop(){
//begin serial
if(Serial.available()>0){
while(Serial.available()>0){
//串口接收
usart_rx_buffer[usart_rx_buffer_pos]=Serial.read();
delay(2);
usart_rx_buffer_pos++;
}
//转发命令
#ifdef DEBUG
Serial.printf("Rec:");
for (int i = 0; i < usart_rx_buffer_pos; i++){
Serial.printf("%c",usart_rx_buffer[i]);
}
Serial.printf("\n");
#endif
//解析串口数据
process_usart_buffer();
//重置接收
usart_rx_buffer_pos=0;
memset(usart_rx_buffer,0,BUFFER_SIZE);
}//end uart scan
//begin MySerial
if(MySerial.available()>0){
while(MySerial.available()>0){
//串口接收
usart_rx_buffer[usart_rx_buffer_pos]=MySerial.read();
delay(2);
usart_rx_buffer_pos++;
}
//解析串口数据
process_usart_buffer();
//重置接收
usart_rx_buffer_pos=0;
memset(usart_rx_buffer,0,BUFFER_SIZE);
}//end uart3 scan
if(digitalRead(sr_in_pin)==LOW){
delay(100);
if(digitalRead(sr_in_pin)==LOW){
Serial.printf("<sr;>");
MySerial.printf("<sr;>");//通知openmv暂停
buzzer_fast();
}
}//end key
if(digitalRead(sl_in_pin)==LOW){
delay(100);
if(digitalRead(sl_in_pin)==LOW){
Serial.printf("<sl;>");
MySerial.printf("<sl;>");//通知openmv开始
buzzer_fast();
}
}//end key
if(digitalRead(key_pin)==LOW){
delay(100);
if(digitalRead(key_pin)==LOW){
//Serial.printf("key");
//MySerial.printf("key");//通知openmv开始
buzzer_fast();
}
}//end key
}
/*
* @功能:解析命令
示例:<xyz x y w h>,缩放角度
命令示例:<mov 20,30,100,200;>
*/
void process_usart_buffer(void){
//分离< >内的内容
char *left_quote_pos = strstr((char *)usart_rx_buffer, "<");
char *right_quote_pos = strstr((char *)usart_rx_buffer, ">");
char *semicolon_pos = strstr((char *)usart_rx_buffer, ";");
if (left_quote_pos != NULL && right_quote_pos != NULL) {
char cmd_length = semicolon_pos - (char*)usart_rx_buffer;
uint8_t cmd[cmd_length];
memcpy(cmd, usart_rx_buffer+1, cmd_length);
cmd[cmd_length - 1] = '\0';
//分析命令
servo_cmd_decode_xyz2polar(cmd);
}
}
/*
@功能:解析命令并转换
*/
void servo_cmd_decode_xyz2polar(uint8_t * cmd){
// 定义目标位置的x,y,画面宽度,画面高度
int current_x, current_y, target_x,target_y;
int angle_polar,angle_z;
//已经去掉“;”了
char *tokens[5]; // 假设最多有5个标记
char *token;
int count = 0;
// 使用strtok函数分割字符串
token = strtok((char*)cmd, " ,");
while (token != NULL) {
tokens[count++] = token;
token = strtok(NULL, " ,");
}
// 判断tokens[0]=="mov"
if(strcmp(tokens[0],"mov") == 0){
// 检查是否有足够的标记以便进行转换
if (count >= 5) {
current_x = atoi(tokens[1]);
current_y = atoi(tokens[2]);
target_x = atoi(tokens[3]);
target_y = atoi(tokens[4]);
#ifdef DEBUG
Serial.printf("CMD:");
Serial.printf("%s(%d,%d,%d,%d)",tokens[0],current_x,current_y,target_x,target_y);
Serial.printf("\n");
#endif
//_25kg:0~270度:0~target_width
//_20kg:0~180度:0~target_height
int target_delta_x = abs(target_x - current_x);
int target_delta_y = abs(target_y - current_y);
angle_polar = (int)(delta_x_coefficient * target_delta_x);
angle_z = (int)(delta_y_coefficient * target_delta_y);
servo_polar(angle_polar,angle_z);//移动云台(角度)
}
}
//判断token[0]=='fix'
if(strcmp(tokens[0],"fix") == 0){
int x1,x2,y1,y2;
// 检查是否有足够的标记以便进行转换
if (count >= 5) {
x1 = atoi(tokens[1]);
y1 = atoi(tokens[2]);
x2 = atoi(tokens[3]);
y2 = atoi(tokens[4]);
#ifdef DEBUG
Serial.printf("CMD:");
Serial.printf("%s(%d,%d,%d,%d)",tokens[0],x1,y1,x2,y2);
Serial.printf("\n");
#endif
delta_x_coefficient=abs(x2-x1)/45.0;
delta_y_coefficient=abs(y2-y1)/45.0;
#ifdef DEBUG
Serial.printf("delta_x_coe:%.2f",delta_x_coefficient);
Serial.printf("\n");
Serial.printf("delta_y_coe:%.2f",delta_y_coefficient);
Serial.printf("\n");
#endif
}
}
//判断token[0]=='angle'
//直接移动到指定角度
if(strcmp(tokens[0],"angle") == 0){
int _angle_polar,_angle_z;
// 检查是否有足够的标记以便进行转换
if (count >= 3) {
_angle_polar = atoi(tokens[1]);
_angle_z = atoi(tokens[2]);
#ifdef DEBUG
Serial.printf("CMD:");
Serial.printf("%s(%d,%d)",tokens[0],_angle_polar,_angle_z);
Serial.printf("\n");
#endif
#ifdef DEBUG
buzzer_fast();
delay(200);
buzzer_fast();
#endif
servo_polar(_angle_polar,_angle_z);
}
}
//判断token[0]=='angle_delta'
//直接移动指定角度(相对)
if(strcmp(tokens[0],"angle_delta") == 0){
int delta_angle_polar,delta_angle_z;
// 检查是否有足够的标记以便进行转换
if (count >= 3) {
delta_angle_polar = atoi(tokens[1]);
delta_angle_z = atoi(tokens[2]);
#ifdef DEBUG
Serial.printf("CMD:");
Serial.printf("%s(%d,%d)",tokens[0],delta_angle_polar,delta_angle_z);
Serial.printf("\n");
#endif
}
}
//判断token[0]=='do':buzz,sl,sr,laser
//蜂鸣器响
if(strcmp(tokens[0],"do") == 0){
// 检查是否有足够的标记以便进行转换
if (count >= 2) {
#ifdef DEBUG
Serial.printf("CMD:");
Serial.printf("%s %s",tokens[0],tokens[1]);
Serial.printf("\n");
#endif
if(strcmp("buzz",tokens[1])==0){
buzzer_fast();
}
if(strcmp("sl",tokens[1])==0){
mouse_send("sl");//通知电脑sl
buzzer_fast();
}
if(strcmp("sr",tokens[1])==0){
mouse_send("sr");//通知电脑sr
buzzer_fast();
}
if(strcmp("laser",tokens[1])==0){
laser();
}
}
//判断token[0]=='ack'
//判断命令响应
if(strcmp(tokens[0],"ack") == 0){
#ifdef DEBUG
Serial.printf("CMD:");
Serial.printf("%s!",tokens[0]);
Serial.printf("\n");
#endif
uart_ack=1;//已响应
}
}//end count>2
}
/*
@功能:根据角度移动两个舵机
*/
void servo_polar(int angle_polar,int angle_z){
//限位,防止幅度过大撞到其他设备(云台上面安装有激光笔).
if (angle_polar < 0) {
angle_polar = 0;
}
else if(angle_polar == 361){
//break;
}
else if (angle_polar > 180 && angle_polar != 361) {
angle_polar = 180;
}
if (angle_z < 35) {
angle_z = 35;
}
else if(angle_z == 361){
//break
}
else if (angle_z > 70 && angle_z != 361) {
angle_z = 70;
}
#ifdef DEBUG
Serial.printf("mov_polar(%d,%d)",angle_polar,angle_z);
Serial.printf("\n");
#endif
servo_angle(angle_polar,1);//移动底部舵机
servo_angle(angle_z,2);//移动顶部舵机
//通知openmv已完成
MySerial.printf("ack");
}
/*
@功能:舵机云台开机初始化校准
*/
void servo_check(void){
//归零
//servo_bottom.writeMicroseconds(1.5*1000);
servo_angle(90,SERVO_BOTTOM);
angle_polar_pos += 90;
delay(15);
//servo_top.writeMicroseconds(0.5*1000);
servo_angle(45,SERVO_TOP);
angle_z_pos -= 90;
delay(15);
//done
buzzer_fast();
MySerial.printf("done");
}
/*
@功能:蜂鸣器短鸣
*/
void buzzer_buzz(void){
pinMode(buzzer_pin,OUTPUT);
digitalWrite(buzzer_pin,HIGH);
delay(500);
digitalWrite(buzzer_pin,LOW);
}
/*
@功能:测试斜线
*/
void test_servo_line(void){
//移动到左下角
servo_top.writeMicroseconds(1.5*1000);
delay(500);
//移动到右上角
servo_bottom.writeMicroseconds(1.83*1000);
delay(500);
servo_top.writeMicroseconds(0.5*1000);
delay(500);//180
servo_top.writeMicroseconds(0.5*1000);
delay(500);//180
}
/*
@功能:测试mpu6050
*/
void mpu6050_test(void){
sensors_event_t a, g, temp;
mpu6050.getEvent(&a, &g, &temp);
/* Print out the values */
Serial.print("Acceleration X: ");
Serial.print(a.acceleration.x);
Serial.print(", Y: ");
Serial.print(a.acceleration.y);
Serial.print(", Z: ");
Serial.print(a.acceleration.z);
Serial.println(" m/s^2");
Serial.print("Rotation X: ");
Serial.print(g.gyro.x);
Serial.print(", Y: ");
Serial.print(g.gyro.y);
Serial.print(", Z: ");
Serial.print(g.gyro.z);
Serial.println(" rad/s");
Serial.print("Temperature: ");
Serial.print(temp.temperature);
Serial.println(" degC");
Serial.println("");
delay(500);
}
/*
@功能:蜂鸣器超短鸣
*/
void buzzer_fast(void){
pinMode(buzzer_pin,OUTPUT);
digitalWrite(buzzer_pin,HIGH);
delay(100);
digitalWrite(buzzer_pin,LOW);
}
/*
@功能:映射值
*/
float servo_map(float value, float fromLow, float fromHigh, float toLow, float toHigh){
// 将value从[fromLow, fromHigh]映射到[toLow, toHigh]
float result = toLow + (value - fromLow) * (toHigh - toLow) / (fromHigh - fromLow);
return result;
}
/*
@功能:开启/关闭激光
*/
void laser(void){
pinMode(laser_pin,OUTPUT);
digitalWrite(laser_pin,LOW);
delay(15);
}
/*
@功能:鼠标发送左/右信号
*/
void mouse_send(char * key){
//左键
if(strcmp(key,"sl")==0){
//开启继电器
pinMode(sl_out_pin,OUTPUT);
digitalWrite(sl_out_pin,HIGH);
delay(100);
//关闭继电器
pinMode(sl_out_pin, OUTPUT); // 启用开漏以保持按钮在未按下时为高电平
digitalWrite(sl_out_pin,LOW);
buzzer_fast();
delay(20);
}
//右键
if(strcmp(key,"sr")==0){
//开启继电器
pinMode(sr_out_pin,OUTPUT);
digitalWrite(sr_out_pin,HIGH);
delay(100);
//关闭继电器
pinMode(sr_out_pin,OUTPUT);
digitalWrite(sr_out_pin,LOW);
buzzer_fast();
delay(200);
buzzer_fast();
delay(800);
}
}
/*
@功能:根据角度移动舵机
*/
void servo_angle(int angle,int servo_number){
//优先使用表格内容
float pulse_servo=0.1;
switch(angle){
case 361:return;break;//什么也不做
case 0:{pulse_servo=0.5;Serial.printf("ok\n");break;};
case 45:{pulse_servo=1.0;break;};
case 90:{pulse_servo=1.5;break;};
case 135:{pulse_servo=2.0;break;};
case 180:{pulse_servo=2.5;break;};
default:{
pulse_servo = servo_map((float)angle, 0, 180.0, 0.5, 2.5);
}//end default
}
// 将角度映射到脉冲宽度
if(pulse_servo != 0.1){
if(servo_number==1) servo_bottom.writeMicroseconds(pulse_servo * 1000);
if(servo_number==2) servo_top.writeMicroseconds(pulse_servo * 1000);
}
delay(50);
#ifdef DEBUG
Serial.printf("pulse(%.2f,servo:%d)",pulse_servo ,servo_number);
Serial.printf("\n");
#endif
}
