舵机效果演示视频
目录
1 Arduino 舵机
接线图：
?执行代码：
#include <Servo.h>
#define ServoPin 3
Servo baseServo;
int angle_s;
int sign;
void setup() {
baseServo.attach(ServoPin); // 初始化舵机
angle_s = 180;
baseServo.write(angle_s);
sign=1;
}
void loop() {
while(true){
baseServo.write(angle_s);
angle_s+=1;
}
while (true) {
baseServo.write(angle_s); // 舵机转动到angle角度
Serial.print("angle: "); // 在控制台打印出 angle: 180\n
Serial.println(angle_s);
angle_s += sign; // 角度增加或减少
if (angle_s >= 180 || angle_s <= 0) { // 当angle转到180°时，i=-1，开始递减;
sign *= -1; //sign=sign*-1 // 当angle转到0°时， i=1， 开始递增;
}
}
}
2 Arduino 超声波传感器
#include <SPI.h>
#define trigPin 6 //超声波模块的Trig 6#
#define echoPin 5 //超声波模块的echo 5#
// 超声波模块
long duration;
int distance;
int calculateDistance(){
//超声波测距后，将距离换算成厘米数，反回。
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance= duration*0.034/2;
return distance;
}
void setup() {
pinMode(trigPin, OUTPUT); //超声波模块发射
pinMode(echoPin, INPUT); //超声波模块接收
Serial.begin(9600);
}
void loop() {
distance = calculateDistance();
Serial.println(distance);
}
3 舵机+测距传感器
//#include <SPI.h>
#include <Servo.h>
// 超声波模块
#define trigPin 6 //超声波模块的Trig 6#
#define echoPin 5 //超声波模块的echo 5#
long duration;
int d;
// 舵机模块
#define ServoPin 3 //底座舵机端口 3#
Servo myServo;
int angle_;
int i = 1;
void setup() {
Serial.begin(9600);
myServo.attach(ServoPin); //初始化舵机
pinMode(trigPin, OUTPUT); //超声波模块发射
pinMode(echoPin, INPUT); //超声波模块接收
}
void loop() {
while (1) {
delay(300);
// 开启舵机
myServo.write(angle_);
Serial.print("a:");
Serial.print(angle_);
Serial.print(";");
// 计算距离
d = calculateDistance();
Serial.print("d:");
Serial.println(d);
angle_ += i;
if (angle_ >= 180 || angle_ <= 0) {
i *= -1;
}
}
}
int calculateDistance() {
//超声波测距后，将距离换算成厘米数，反回。
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
d = duration * 0.034 / 2;
return d;
}
4 Python 绘制动态雷达
from plot8 import sub_plot_c
import random
pl = sub_plot_c()
for i in range(0,180):
d = random.randint(0, 40)
pl.update(angle=i, c=45, Clockwise=False, alpha=0.1, distance=d)
pl.draw_img()
pl.clear()
for i in range(180,0,-1):
d = random.randint(0, 40)
pl.update(angle=i, c=45, Clockwise=True, alpha=0.1, distance=d)
pl.draw_img()
pl.clear()
from matplotlib import colors, pyplot as plt
import math
import numpy as np
import random
# 目标：每扫描一个角度的点， 替换该点， 其他的保留
class sub_plot_c:
def __init__(self):
self.x = 0
self.x_list = []
self.y_list = []
# 底线高度
self.base_height = 0
# 雷达图半径长度
self.r_length = 800
self.fig = plt.figure(figsize=(16, 10)) # 1500 * 1000 w = 1.5h
self.n = 1500 / 1000
self.angle = 20
self.c = 30
self.Clockwise = False # 逆时针 pre在左， 顺时针 pre在右
# 扫描透明度
self.alpha = 1.0
# 图像背景
self.backgroud_inside = "#0A2B56"
self.backgroud_outside = "#1675CC"
self.line_color = 'white'
self.sector = 'white'
# 标记点类型
self.mark_s = ['o','<','>','s','p','*','+','D','d']
self.mark_col = ['#E9E7D0', '#49AED5', 'blue']
# 每个角度上存储一种标记
self.mark_dict = {} #[angle, [distance, mark_s, mark_c,]]
self.current_distance = 30
self.max_distance = 30
self.mark_scale = self.r_length / self.max_distance
self.count = 0
self.ax = self.fig.add_subplot()
plt.ion()
def update(self, angle, c, Clockwise, alpha, distance):
self.angle = angle
self.c = c
self.Clockwise = Clockwise
self.alpha = alpha
self.current_distance = distance
# 交由外部清空
def clear(self):
plt.pause(0.1) # 动画延迟
plt.ioff() # 关闭画图的窗口
plt.clf()
def draw_img(self):
# 画固定图形
# 底线
plt.plot([-self.r_length, self.r_length], [self.base_height, self.base_height], color=self.line_color) # x1,x2 y1,y2
# 半圆, 弧度0-pi, 整圆， 弧度0-2*pi
theta = np.linspace(0, math.pi,180*3)
# 2*pi*r, x,y为数组
x,y = np.cos(theta)*self.r_length, np.sin(theta)*self.r_length
y += self.base_height
# plt.plot(x, y, color='black')
# 画外部背景
x_bg = np.linspace(-self.r_length, self.r_length, 100)
y_circle_bg = np.sqrt(1-(x_bg/self.r_length)*(x_bg/self.r_length)) * self.r_length
y_circle_bg += self.base_height
# 画内圆背景
plt.fill_between(x_bg, self.base_height, y_circle_bg, facecolor=self.backgroud_inside, interpolate=True)
# 画外圆线1
plt.plot(x, y, color=self.line_color, linewidth=1)
# 画外圆线2
x_2,y_2 = np.cos(theta)*self.r_length*0.8, np.sin(theta)*self.r_length*0.8
y_2 += self.base_height
plt.plot(x_2, y_2, color=self.line_color, linewidth=1)
# 画外圆线3
x_3,y_3 = np.cos(theta)*self.r_length*0.6, np.sin(theta)*self.r_length*0.6
y_3 += self.base_height
plt.plot(x_3, y_3, color=self.line_color, linewidth=1)
# 画外圆线4
x_4,y_4 = np.cos(theta)*self.r_length*0.4, np.sin(theta)*self.r_length*0.4
y_4 += self.base_height
plt.plot(x_4, y_4, color=self.line_color, linewidth=1)
# 画外圆线5
x_5,y_5 = np.cos(theta)*self.r_length*0.4, np.sin(theta)*self.r_length*0.4
y_5 += self.base_height
plt.plot(x_5, y_5, color=self.line_color, linewidth=1)
######################################## 画扇形 start ###################################################
# pre, end两条线， 前后线在来回摆动的过程中会发生位置的互换, # 图示标出
x_pre_angle = round(math.cos(math.radians(self.angle)),2)
y_pre_angle = round(math.sin(math.radians(self.angle)),2)
# 扇形角度
angle_sec = self.angle + self.c if self.Clockwise else self.angle - self.c
x_end_angle = round(math.cos(math.radians(angle_sec)),2)
y_end_angle = round(math.sin(math.radians(angle_sec)),2)
# 五种状态:
# 1. pre在第二象限， end在第一象限, 扇形面积 = (圆-pre) - (end-base_height);
# 2. pre,end都在第二象限, 扇形面积 = (圆-pre) - (圆-end);
# 3. pre,end都在第一象限, 扇形面积 = (圆-pre) - (圆-end);
# 4. end 与 90°重合, 扇形面积 = (圆-pre)
# 5. pre 与 90°重合, 扇形面积 = (圆-end)
x_pre = x_pre_angle * self.r_length
y_pre = y_pre_angle * self.r_length
x_end = x_end_angle * self.r_length
y_end = y_end_angle * self.r_length
# 构建填充数据集
if self.Clockwise :
if self.angle + self.c <= 180: # 解决夹角过小，无法填充满的问题
x_left = x_end
x_right = x_pre
else:
x_left = -self.r_length
x_right = x_pre
else:
if self.angle >= self.c:
x_left = x_pre
x_right = x_end
else:
x_left = x_pre
x_right = self.r_length
if max([x_pre, x_end]) < 0:
x_right = 0
elif min([x_pre, x_end]) > 0:
x_left = 0
x = np.linspace(x_left, x_right, 100)
# x=sqrt(1-sin(theta)^2)*r (x/r)^2 = 1 - sin(theta)^2 sin(theta)^2 = 1-(x/r)^2 sin(theta) = sqrt(1-(x/r)^2) y=sqrt(1-(x/r)^2)*r
# pre边的斜率
if x_pre != 0:
k_pre_line = ((y_pre + self.base_height) -(0 + self.base_height)) / (x_pre-0)
y_pre_line = k_pre_line * x
y_pre_line += self.base_height
# 圆周的描点
# x=sqrt(1-sin(theta)^2)*r (x/r)^2 = 1 - sin(theta)^2 sin(theta)^2 = 1-(x/r)^2 sin(theta) = sqrt(1-(x/r)^2) y=sqrt(1-(x/r)^2)*r
y_circle = np.sqrt(1-(x/self.r_length)*(x/self.r_length)) * self.r_length
# end边的斜率
if x_end != 0:
k_end_line = ((y_end+self.base_height) -(0+self.base_height)) / (x_end-0)
y_end_line = k_end_line * x
y_end_line += self.base_height
# 加上向上偏移
y_circle += self.base_height
x_temp = x - 1 if self.Clockwise else x + 1
# 1.
if (x_pre_angle < 0 and x_end_angle > 0) or (x_pre_angle > 0 and x_end_angle < 0):
plt.fill_between(x, y_pre_line, y_circle, facecolor=self.sector, interpolate=True, alpha = self.alpha)
plt.fill_between(x_temp, self.base_height, y_end_line, facecolor=self.backgroud_inside,interpolate=True)
# 2.
elif x_pre_angle < 0 and x_end_angle < 0:
plt.fill_between(x, self.base_height, y_circle, facecolor=self.sector, interpolate=True, alpha = self.alpha)
if self.Clockwise:
x_temp_2 = x + 1
plt.fill_between(x_temp_2, y_pre_line, y_circle, facecolor=self.backgroud_inside,interpolate=True)
plt.fill_between(x_temp, self.base_height, y_end_line, facecolor=self.backgroud_inside,interpolate=True)
else:
x_temp_2 = x - 1
plt.fill_between(x_temp, y_end_line, y_circle, facecolor=self.backgroud_inside,interpolate=True)
plt.fill_between(x_temp_2, self.base_height, y_pre_line, facecolor=self.backgroud_inside,interpolate=True)
# 3.
elif x_pre_angle > 0 and x_end_angle > 0:
plt.fill_between(x, self.base_height, y_circle, facecolor=self.sector, interpolate=True, alpha = self.alpha)
if self.Clockwise:
x_temp_2 = x + 1
plt.fill_between(x_temp_2, self.base_height, y_pre_line, facecolor=self.backgroud_inside,interpolate=True)
plt.fill_between(x_temp, y_end_line, y_circle, facecolor=self.backgroud_inside,interpolate=True)
else:
x_temp_2 = x - 1
plt.fill_between(x_temp, self.base_height, y_end_line, facecolor=self.backgroud_inside,interpolate=True)
plt.fill_between(x_temp_2, y_pre_line, y_circle, facecolor=self.backgroud_inside,interpolate=True)
# 4.
elif x_end_angle == 0:
plt.fill_between(x, y_pre_line, y_circle, facecolor=self.sector, interpolate=True, alpha = self.alpha)
# 5.
elif x_pre_angle == 0:
plt.fill_between(x, y_end_line, y_circle, facecolor=self.sector, interpolate=True, alpha = self.alpha)
print(x_pre_angle, x_end_angle, )
plt.fill_between(x, 0, self.base_height, facecolor=self.backgroud_outside, interpolate=True)
######################################## 画扇形 end ###################################################
# 画外圆背景
plt.fill_between(x_bg, y_circle_bg, 1000, facecolor=self.backgroud_outside, interpolate=True)
# 画下面的背景
plt.fill_between(x_bg, 0, self.base_height, facecolor=self.backgroud_outside, interpolate=True)
# 画刻度表示
plt.fill_between(x_bg, y_circle_bg, 1000, facecolor=self.backgroud_outside, interpolate=True)
# 画数据标识
plt.fill_between(x_bg, 0, self.base_height, facecolor=self.backgroud_outside, interpolate=True)
# 画刻度线标识 从0到180度， 每20°标记一下
for angle_text in range(0, 181, 20):
x_text = round(math.cos(math.radians(angle_text)),2) * self.r_length
y_text = round(math.sin(math.radians(angle_text)),2) * self.r_length
x_text -= 8
y_text += self.base_height + 2
plt.text(x_text, y_text, str(angle_text),fontsize=12, color = '#38C0B0')
# 记录数据
plt.text(-self.r_length, 950, 'Randar Scan: ',fontsize=25, color = 'white')
plt.text(-self.r_length + 340, 950, 'ON',fontsize=25, color = 'white')
plt.text(-self.r_length, 900, 'Pluse: ',fontsize=20, color = 'white')
plt.text(-self.r_length + 160, 900, 'S2',fontsize=20, color = 'white')
plt.text(-self.r_length, 860, 'current_angle: ',fontsize=15, color = 'white')
plt.text(-self.r_length + 270, 860, str(self.angle),fontsize=15, color = 'white')
plt.text(-self.r_length, 820, 'current_distance: ',fontsize=15, color = 'white')
plt.text(-self.r_length + 270, 820, str(self.current_distance),fontsize=15, color = 'white')
plt.text(-self.r_length, 780, 'current_count: ',fontsize=15, color = 'white')
plt.text(-self.r_length + 270, 780, str(self.angle),fontsize=15, color = 'white')
# 画标记点
s = random.randint(0, len(self.mark_s)-1)
c = random.randint(0, len(self.mark_col)-1)
# plt.plot(100, 100, self.mark_s[s], color=self.mark_col[c])
############################# 根据距离画点 start ######################################
# 如果当前点的距离没发生变化， 则不更新
# 如果当前点的距离大于30，则画空值， 如果小于30，则位置信息全部更新
#[angle, [distance, mark_s, mark_c,]]
if self.angle not in self.mark_dict.keys():
if self.current_distance <= self.max_distance:
self.mark_dict[self.angle] = [self.current_distance, self.mark_s[s], self.mark_col[c]]
self.count += 1
else:
self.mark_dict[self.angle] = [0, self.mark_s[s], self.mark_col[c]]
if self.mark_dict[self.angle][0] != self.current_distance:
if self.current_distance > self.max_distance:
self.mark_dict[self.angle] = [self.current_distance, "", self.mark_col[c]]
self.count -= 1
else:
self.mark_dict[self.angle] = [self.current_distance, self.mark_s[s], self.mark_col[c]]
if self.mark_dict[self.angle][0]:
self.count +=1
# 打印出所有点
for k,v in self.mark_dict.items():
x_mark = round(math.cos(math.radians(k)),2) * v[0] * self.mark_scale
y_mark = round(math.sin(math.radians(k)),2) * v[0] * self.mark_scale
plt.plot(x_mark, y_mark, v[1], color=v[2])
plt.axis('off')
plt.xlim(-self.r_length, self.r_length)
plt.ylim(0, 1000)
def release():
plt.show()