2023-03-05 超声波模块
之所以要使用超声波雷达模块，是因为依靠视觉识别模块难以进行距离的判断。虽然可以通过算法实现，但是难度大，准确度不高。
超声波模块便宜、教程多、上手简单，可以用作初期的测试。
2023-04-04 实现双模块同时工作
使用了 mbed 库中的 ticker 接口（(61条消息) Mbed OS 文档翻译 之 参考（API（驱动（Ticker）））
通过使用 ticker 接口，创造一个 ticker 对象。ticker 对象能够设置定期中断。通过使用 attach 函数能够将另外一个函数对象附加到 ticker 对象之上。这样就可以让另外的这个函数对象实现 ticker 对象的功能，即设置定期的中断，以指定的速率重复调用。
2023-04-05 ticker 的使用问题
现在遇到了一个逻辑上的问题。程序现在开始报错，完全无法正常运行
++ MbedOS Error Info ++
Error Status: 0x80010133 Code: 307 Module: 1
Error Message: Mutex: 0x2000226C, Not allowed in ISR context
Location: 0x8009AAF
Error Value: 0x2000226C
Current Thread: rtx_idle Id: 0x20001AE0 Entry: 0x8007571 StackSize: 0x300 StackMem: 0x20001B68 SP: 0x2000FEF4
For more info, visit: https://mbed.com/s/error?error=0x80010133&tgt=NUCLEO_L432KC
-- MbedOS Error Info --
不是很能明白什么是 ISR、Mutex。
2023-04-06 Rtos Mutex Error
Github 上查到有人遇到过相似的问题。可能是 Serial 函数的冲突问题，可以通过使用 RawSerial 来解决。
Mutex lock failed on Mbed OS5.10.x · Issue #8518 · ARMmbed/mbed-os --- Mbed OS5.10.x 上的互斥锁失败
已解决，不再报错，能正常输出。
但是现在还是无法正常输出。
distance1 = getDistance(echo1);
pc.printf("distance1 HAS BEEN calculated");
distance2 = getDistance(echo2);
pc.printf("distance2 HAS BEEN calculated");// 计算距离的函数
float getDistance(DigitalIn &echo) {
//echo==0时，不断循环（等待状态），echo!=0就跳出循环
while (echo == 0);
//echo==1时，不断进行时间的读取。如果时间大于阈值，则探测的距离超出量程
//echo==0时，跳出循环
while (echo != 0) {
pulse_width = timer.read_us();
if (pulse_width > 23529) {
// 超时，距离太远，返回0
return 0;
}
}
//读取所花费的时间
pulse_width=timer.read();
pc.printf("pulse_width= %.2f\n",pulse_width);
// 计算距离
float distance = pulse_width * 340.0 * 50.0;
return distance;
}出问题的应该就是这两段代码，把第一段代码注释掉就能有输出（输出为 0），如果不注释掉就无法正确输出。
2023 -04-07 时序调整
考虑整个程序中关键的 timer 在不同的函数进行了不同频率的调用，有可能是在主程序中的时序不齐导致了无输出或者乱输出（大于 10m）
我们把 trig_send 和 getDistance 两个函数合并。
但是有报错 Cannot take the address of an rvalue of type 'void'
attach() 的函数参数必须是一个无参函数指针，所以不能像之前那样使用带参数的函数指针。因此，你需要将 getDistance() 函数的参数移动到全局变量中，以便 ticker.attach() 可以调用无参函数。
#include "mbed.h"
// 定义HC-SR04模块的引脚
DigitalIn echo1(D9);
DigitalOut trig1(D10);
DigitalIn echo2(D4);
DigitalOut trig2(D5);
// 定义计时器
Ticker ticker;
Timer timer,k;
RawSerial pc(USBTX, USBRX);
volatile float pulse_width = 0;
float distance1 = 0, distance2 = 0;
// 计算距离的函数
void getDistance() {
// 计算第一个 HC-SR04 的距离
timer.reset();
trig1 = 0;
trig1 = 1;
wait_us(10);
trig1 = 0;
timer.start();
while (echo1 == 0);
while (echo1 == 1) {
pulse_width = timer.read_us();
if (pulse_width > 23529) {
distance1 = 0;
return;
}
}
timer.stop();
pulse_width = timer.read();
distance1 = pulse_width * 340.0 * 50.0;
// 计算第二个 HC-SR04 的距离
timer.reset();
trig2 = 0;
trig2 = 1;
wait_us(10);
trig2 = 0;
timer.start();
while (echo2 == 0);
while (echo2 == 1) {
pulse_width = timer.read_us();
if (pulse_width > 23529) {
distance2 = 0;
return;
}
}
timer.stop();
pulse_width = timer.read();
distance2 = pulse_width * 340.0 * 50.0;
}
int main() {
pc.baud(9600);
pc.printf("---THE PROGRAMME HAS BEEN RESTARTED---\n\n");
// 初始化定时器
k.reset();
k.start();
// 注册 Ticker
ticker.attach(&getDistance, 0.05);
while (true) {
pc.printf("Distance1: %.2f cm, Distance2: %.2f cm\n", distance1, distance2);
wait(1);
}
}
能跑了，但是不知道为什么有 37cm 左右的误差
#include "mbed.h"
// 定义HC-SR04模块的引脚
DigitalIn echo1(D4);
DigitalOut trig1(D5);
DigitalIn echo2(D9);
DigitalOut trig2(D10);
// 定义计时器
Ticker ticker;
Timer timer,k;
RawSerial pc(USBTX, USBRX);
volatile float pulse_width = 0;
float distance1 = 0, distance2 = 0;
// 计算距离的函数
void getDistance() {
// 计算第一个 HC-SR04 的距离
timer.reset();
trig1 = 0;
trig1 = 1;
wait_us(10);
trig1 = 0;
timer.start();
while (echo1 == 0);
while (echo1 == 1) {
pulse_width = timer.read_us();
if (pulse_width > 23529 || pulse_width < 0.58) {
distance1 = 0;
return;
}
}
timer.stop();
distance1 = timer.read() * 340.0 * 50.0-38;
// 计算第二个 HC-SR04 的距离
timer.reset();
trig2 = 0;
trig2 = 1;
wait_us(10);
trig2 = 0;
timer.start();
while (echo2 == 0);
while (echo2 == 1) {
pulse_width = timer.read_us();
if (pulse_width > 23529 || pulse_width < 0.58) {
distance2 = 0;
return;
}
}
timer.stop();
pulse_width = timer.read();
distance2 = pulse_width * 340.0 * 50.0-37.58;
}
int main() {
pc.baud(9600);
pc.printf("---THE PROGRAMME HAS BEEN RESTARTED---\n\n");
// 初始化定时器
k.reset();
k.start();
// 注册 Ticker
ticker.attach(&getDistance, 0.05);
while (true) {
pc.printf("Distance1: %.2f cm, Distance2: %.2f cm\n", distance1, distance2);
wait_us(1e6);
}
}2023-04-08 优化输出
现在输出有两个问题：
- 输出不稳定
- 有约 37.58cm 的固定误差
- 在实际距离小于 4cm 的时候，无法正常测量
2023-04-22 无法烧录程序
今天测试了超声波在 patio2 的程序
这次使用了 mbed 库里的 HCSR04 库
发生了以下的问题：
- 在左转函数后，判断条件出了兼容性问题
- 通过对判断条件加绝对值函数解决了问题
- 无法烧录程序，可能是 G474 通病？或许是固件的问题？
#include "mbed.h"
#include "HCSR04.h"
/* 电机引脚设置 */
DigitalOut AIN_1(PA_9);
DigitalOut AIN_2(D9);
DigitalOut BIN_1(PB_5);
DigitalOut BIN_2(PA_10);
DigitalOut STBY(D10);
PwmOut PWMA(D7);
PwmOut PWMB(D6);
/* 电机引脚设置 */
/* 定义HC-SR04模块的引脚 */
HCSR04 HCSR04_1(PB_12, PB_11);
HCSR04 HCSR04_2(PB_13, PB_14);
HCSR04 HCSR04_3(PC_11, PC_10);
HCSR04 HCSR04_4(PC_9, PC_8);
/* 定义HC-SR04模块的引脚 */
BufferedSerial pc(USBTX, USBRX, 9600);
int flag=1;
// FileHandle *mbed::mbed_override_console(int fd)
// {
// return &pc;
// }
void gostraight()
{
STBY=1;
PWMA.period(0.0001f);
PWMB.period(0.0001f);
PWMA.write(0.15f);
PWMB.write(0.15f);
AIN_1=0;
AIN_2=1;
BIN_1=0;
BIN_2=1;
}
void gostraight_correct()
{
if((HCSR04_3.getCm()-HCSR04_4.getCm())>3){
PWMA.write(0.15f);
PWMB.write(0.10f);
}
if((HCSR04_3.getCm()-HCSR04_4.getCm())<-3){
PWMA.write(0.10f);
PWMB.write(0.15f);
}
STBY=1;
PWMA.period(0.0001f);
PWMB.period(0.0001f);
AIN_1=0;
AIN_2=1;
BIN_1=0;
BIN_2=1;
}
void turnleft()
{
STBY=1;
PWMA.period(0.0001f);
PWMB.period(0.0001f);
PWMA.write(0.15f);
PWMB.write(0.15f);
AIN_1=1;
AIN_2=0;
BIN_1=0;
BIN_2=1;
}
void turnright()
{
STBY=1;
PWMA.period(0.0001f);
PWMB.period(0.0001f);
PWMA.write(0.15f);
PWMB.write(0.15f);
AIN_1=0;
AIN_2=1;
BIN_1=1;
BIN_2=0;
}
void stop()
{
STBY=0;
PWMA.period(0.0001f);
PWMB.period(0.0001f);
PWMA.write(0.15f);
PWMB.write(0.15f);
AIN_1=0;
AIN_2=1;
BIN_1=0;
BIN_2=1;
}
int main()
{
while (true) {
printf("the distance of US-01 = %.1f\n", HCSR04_1.getCm());
printf("the distance of US-02 = %.1f\n", HCSR04_2.getCm());
printf("the distance of US-03 = %.1f\n", HCSR04_3.getCm());
printf("the distance of US-04 = %.1f\n", HCSR04_4.getCm());
printf("Flag = %d\n\n", flag);
wait_us(1e6);
if(HCSR04_1.getCm()>60 && HCSR04_2.getCm()>60 && HCSR04_3.getCm()>60 && HCSR04_4.getCm()>60 && flag==1){ //也许有问题
gostraight();
}
else if(HCSR04_1.getCm()<12 && HCSR04_2.getCm()<12 && HCSR04_3.getCm()>60 && HCSR04_4.getCm()>60 && flag ==1){
stop();
// wait_us(1e6);
turnleft();
while(1){
if(HCSR04_1.getCm()>60 && HCSR04_2.getCm()>60 && HCSR04_3.getCm()<20 && HCSR04_4.getCm()<20 && abs(HCSR04_3.getCm()-HCSR04_4.getCm())<2){
stop();
wait_us(1e6);
break;
}
}
flag=0;
}
else if(HCSR04_1.getCm()>60 && HCSR04_2.getCm()>60 && HCSR04_3.getCm()<20 && HCSR04_4.getCm()<20 && flag==0){
gostraight_correct(); //PID
}
else if(HCSR04_1.getCm()>60 && HCSR04_2.getCm()>60 && HCSR04_3.getCm()>60 && HCSR04_4.getCm()<20 && flag==0){
gostraight();
wait_us(2.5e6);
}
else if(HCSR04_1.getCm()>60 && HCSR04_2.getCm()>60 && HCSR04_3.getCm()>60 && HCSR04_4.getCm()>60 && flag==0){
turnright();
wait_us(3e6);
gostraight();
wait_us(3e6);
}
else if(HCSR04_1.getCm()<20 && HCSR04_2.getCm()<20 && HCSR04_3.getCm()<20 && HCSR04_4.getCm()<20){
turnleft();
while(1){
if(HCSR04_1.getCm()>60 && HCSR04_2.getCm()>60 && HCSR04_3.getCm()<20 && HCSR04_4.getCm()<20 && (HCSR04_3.getCm()-HCSR04_4.getCm())<2){
stop();
wait_us(1e6);
break;
}
}
}
}
}2023-04-24 在超声波模块中引入深度学习
如果有可能，我想在超声波模块中引入深度学习，就像视觉组所做的那样。我简单了解到 stm32 专门有个工具：STM32Cube.AI 用来辅助开发 AI，但是我不知道能不能在 L432KC 以及 G474RE 上进行开发。
2023-04-26 解决程序无法运行的问题
G474使用的相关问题： 1. 在拷程序的时候，先给主控上电，驱动板不能上电，要不然程序无法正常运行； 2. 在使用主控的时候，不能按复位按钮（黑色那个），也会导致程序不能运行
程序相关问题，不能够在条件判断中不断调用函数，会造成精度下降。
2023-04-29 超声波 patio2
问题：
- 左转判断距离过短
- 增加判断的距离
- 左转平齐判断有误
- 不知道为什么就好了
- 贴边距离过短
- 将前行修正函数的距离改为15cm
- 15cm 不够，得延长至 17，否则车尾在转向后会撞墙
- 贴边右转时，柱子会比栏杆多出 4cm 左右
- 减小每次改正的幅度，延长每次改正的时间
超声波已经能在 patio2 正常运行
2023-05-29 状态旗
- 必须在状态旗前加 volatile 关键字
- 未解决的问题：第一次左转的前面距离太短，不是速度或者距离的问题
- 重新调速度
- 第一次左转后需要 stop2 秒
- 第一次右转时间不够长
2023-06-02 patio 1
#include "mbed.h"
#include "HCSR04.h"
DigitalIn A(PC_12);
DigitalIn B(PD_2);
/* 电机引脚设置 */
DigitalOut AIN_1(PA_9);
DigitalOut AIN_2(D9);
DigitalOut BIN_1(PB_5);
DigitalOut BIN_2(PA_10);
DigitalOut STBY(D10);
PwmOut PWMA(D7);
PwmOut PWMB(D6);
HCSR04 HCSR04_1(PB_12, PB_11);
HCSR04 HCSR04_2(PB_13, PB_14);
HCSR04 HCSR04_3(PC_11, PC_10);
HCSR04 HCSR04_4(PC_9, PC_8);
BufferedSerial pc(USBTX, USBRX,9600);
float distance1=0;
float distance2=0;
float distance3=0;
float distance4=0;
float d34=0;
float d12=0;
volatile int flag_t=0;
volatile int flag=1;
void gostraight(){
STBY=1;
PWMA.period(0.0001f);
PWMB.period(0.0001f);
PWMA.write(0.4f);
PWMB.write(0.413f);
AIN_1=0;
AIN_2=1;
BIN_1=0;
BIN_2=1;
}
void gostraight_correct(){
if(distance3>distance4 && distance4>20){
//向左偏时，向右修正
PWMA.write(0.4f);
PWMB.write(0.35f);
}
if(distance3<distance4 && (distance3<20 || distance4<20)){
//向右偏时，向左修正
PWMA.write(0.32f);
PWMB.write(0.42f);
}
if ((d34)>=-1 && (d34)<=1 ){
//不偏时直走
PWMA.write(0.3f);
PWMB.write(0.3135f);
}
STBY=1;
PWMA.period(0.0001f);
PWMB.period(0.0001f);
AIN_1=0;
AIN_2=1;
BIN_1=0;
BIN_2=1;
wait_us(5e4);
printf("***** CORRECTING *****\n");
}
void gostraight_correct_12(){
if(distance1>distance2){
//向左偏时，向右修正
PWMA.write(0.4f);
PWMB.write(0.35f);
}
if(distance1<distance2){
//向右偏时，向左修正
PWMA.write(0.32f);
PWMB.write(0.42f);
}
if ((d12)>=-1 && (d12)<=1 ){
//不偏时直走
PWMA.write(0.3f);
PWMB.write(0.3135f);
}
STBY=1;
PWMA.period(0.0001f);
PWMB.period(0.0001f);
AIN_1=0;
AIN_2=1;
BIN_1=0;
BIN_2=1;
wait_us(1e4);
printf("***** CORRECTING *****\n");
}
void turnleft(){
STBY=1;
PWMA.period(0.0001f);
PWMB.period(0.0001f);
PWMA.write(0.15f);
PWMB.write(0.15f);
AIN_1=1;
AIN_2=0;
BIN_1=0;
BIN_2=1;
}
void turnright(){
STBY=1;
PWMA.period(0.0001f);
PWMB.period(0.0001f);
PWMA.write(0.20f);
PWMB.write(0.20f);
AIN_1=0;
AIN_2=1;
BIN_1=1;
BIN_2=0;
}
void stop(){
STBY=0;
PWMA.period(0.0001f);
PWMB.period(0.0001f);
PWMA.write(0.15f);
PWMB.write(0.15f);
AIN_1=0;
AIN_2=1;
BIN_1=0;
BIN_2=1;
}
void tracking(){
printf("***** ENTER TRACKING *****");
// while(1){
distance3=HCSR04_3.getCm();
distance4=HCSR04_4.getCm();
printf("the distance of US-03 = %.1f\n", distance3);
printf("the distance of US-04 = %.1f\n", distance4);
printf("Flag = %d\n\n", flag);
//用于巡线
if (A==0 && B==1){
printf("turnleft\n");
turnleft();
}
else if (A==1 && B==0){
printf("turnright\n");
turnright();
}
else if (A==1 && B==1){
printf("gostraight\n");
gostraight();
}
//用于跳出tracking
else if (distance3 >30 && distance4 < 30){
gostraight();
printf("!!!!! ready for turning RIGHT !!!!!\n");
wait_us(0.7e6);
stop();
wait_us(1e6);
}
else if(distance3 > 40 && distance4 > 40){
printf("turning RIGHT >>>>>\n");
turnright();
wait_us(1.8e6);//1.5e6 3&3.135
gostraight();
wait_us(2e6);
}
// }
flag=2;
}
void passbridge(){
while (1){
distance1=HCSR04_1.getCm();
distance2=HCSR04_2.getCm();
distance3=HCSR04_3.getCm();
distance4=HCSR04_4.getCm();
d34=distance3-distance4;
printf("the distance of US-01 = %.1f\n", distance1);
printf("the distance of US-02 = %.1f\n", distance2);
printf("the distance of US-03 = %.1f\n", distance3);
printf("the distance of US-04 = %.1f\n", distance4);
printf("d34 = %.1f\n", d34);
printf("Flag = %d\n\n", flag);
if (distance1>30 && distance2>30 && distance3<60 && distance4<60){//可能桥面会影响
gostraight_correct();
}
else if (distance1>30 && distance2>30){
gostraight();
}
else if (distance1<30 && distance2<30){
stop();
wait_us(1e6);
turnleft();
printf("<<<<< turning LEFT\n");
while(1){
distance3=HCSR04_3.getCm();
distance4=HCSR04_4.getCm();
d34=distance3-distance4;
if(abs(d34)<1){
stop();
printf("===== STOP turning LEFT =====\n");
wait_us(1e6);
break;
}
}
break;
}
}
flag=3;
}
int main()
{
wait_us(3e6);
printf("========4========\n");
while(1){
switch (flag){
case 1: {
printf("11111 tracking 11111\n");
tracking();
flag = 2;
break;
}
case 2: {
printf("22222 bridge passing 22222\n");
passbridge();
flag = 3;
break;
}
case 3: {
printf("33333 The door 33333\n");
gostraight();
wait_us(11e6);
stop();
break;
}
}
}
}- 在运行的时候出现了问题，无法进入 switch-case 1 的 tracking 函数
- 不知道为什么突然好了
- 可能是我先运行了 patio2 的程序？
- 我发现是上电的问题。出现这个问题之后，主板必须全部断电，两个开关断开、线拔掉。然后重新上电。上电顺序为：主板→电机→超声波
- 不知道为什么突然好了
2023-06-03 Patio 2 最终代码
#include "mbed.h"
#include "HCSR04.h"
#include <cstdio>
/* 电机引脚设置 */
DigitalOut AIN_1(PA_9);
DigitalOut AIN_2(D9);
DigitalOut BIN_1(PB_5);
DigitalOut BIN_2(PA_10);
DigitalOut STBY(D10);
PwmOut PWMA(D7);
PwmOut PWMB(D6);
/* 电机引脚设置 */
//与openmv通信
DigitalIn A(PC_12);
DigitalIn B(PD_2);
// int A=0;
// int B=1;
/* 定义HC-SR04模块的引脚 */
HCSR04 HCSR04_1(PB_12, PB_11);
HCSR04 HCSR04_2(PB_13, PB_14);
HCSR04 HCSR04_3(PC_11, PC_10);
HCSR04 HCSR04_4(PC_9, PC_8);
/* 定义HC-SR04模块的引脚 */
/* 定义陀螺仪模块的引脚 */
BufferedSerial gyro(PC_4,PC_5);
/* 定义陀螺仪模块的引脚 */
DigitalOut COM(PC_2);//黑色线
DigitalOut SER(PC_3);//棕色线
float distance1=0;
float distance2=0;
float distance3=0;
float distance4=0;
float d34=0;
float ax,ay,az,gx,gy,gz,roll,pitch,yaw;
volatile int flag_u=1;//超声波标识符
volatile int flag_s=0;//超声波左转标识符
/* 标识符 */
volatile int flag=1;
/* 标识符 */
BufferedSerial pc(USBTX, USBRX,9600);
void data_print(){
distance1=HCSR04_1.getCm();
distance2=HCSR04_2.getCm();
distance3=HCSR04_3.getCm();
distance4=HCSR04_4.getCm();
d34=distance3-distance4;
printf("the distance of US-01 = %.1f\n", distance1);
printf("the distance of US-02 = %.1f\n", distance2);
printf("the distance of US-03 = %.1f\n", distance3);
printf("the distance of US-04 = %.1f\n", distance4);
printf("d34 = %.1f\n", d34);
printf("Flag_u = %d\n\n", flag_u);
printf("Flag_s = %d\n\n", flag_s);
printf("Flag = %d\n\n", flag);
}
void communication(){
COM=1;
}
void droptheball(){
SER=1;
}
void getJY61data(void){
short a[3], w[3], Angle[3], T;
uint8_t chrTemp[256];
static unsigned short RxLength = 0;
uint8_t stcAcc[8],stcGyro[8],stcAngle[8];
uint8_t chrBuffer[256];
unsigned short Length=0,Cnt=0;
while(gyro.readable()){
gyro.read(&chrBuffer[Length++],1);
}
if (Length >= sizeof(chrBuffer)) {
Length = 0;
//function
RxLength = 0;
memcpy(chrTemp,chrBuffer,sizeof(chrBuffer));
RxLength += sizeof(chrBuffer);
while (RxLength >= 11)
{
if (chrTemp[0] != 0x55)
{
RxLength--;
memcpy(&chrTemp[0],&chrTemp[1],RxLength);
continue;
}
switch(chrTemp[1])
{
case 0x51: memcpy(&stcAcc,&chrTemp[2],8);
a[0] = (short)((short)(stcAcc[1]) << 8 | stcAcc[0]);
a[1] = (short)((short)(stcAcc[3]) << 8 | stcAcc[2]);
a[2] = (short)((short)(stcAcc[5]) << 8 | stcAcc[4]);
break;
case 0x52: memcpy(&stcGyro,&chrTemp[2],8);
w[0] = (short)((short)(stcGyro[1]) << 8 | stcGyro[0]);
w[1] = (short)((short)(stcGyro[3]) << 8 | stcGyro[2]);
w[2] = (short)((short)(stcGyro[5]) << 8 | stcGyro[4]);
break;
case 0x53: memcpy(&stcAngle,&chrTemp[2],8);
Angle[0] = (short)((short)(stcAngle[1]) << 8 | stcAngle[0]);
Angle[1] = (short)((short)(stcAngle[3]) << 8 | stcAngle[2]);
Angle[2] = (short)((short)(stcAngle[5]) << 8 | stcAngle[4]);
break;
//default: break;
}
RxLength -= 11;
memcpy(&chrTemp[0],&chrTemp[11],RxLength);
}
}
else if (Length>0)
{
RxLength = 0;
//function
memcpy(chrTemp,chrBuffer,Length);
RxLength += Length;
while (RxLength >= 11)
{
if (chrTemp[0] != 0x55)
{
RxLength--;
memcpy(&chrTemp[0],&chrTemp[1],RxLength);
continue;
}
switch(chrTemp[1])
{
case 0x51: memcpy(&stcAcc,&chrTemp[2],8);
a[0] = (short)((short)(stcAcc[1]) << 8 | stcAcc[0]);
a[1] = (short)((short)(stcAcc[3]) << 8 | stcAcc[2]);
a[2] = (short)((short)(stcAcc[5]) << 8 | stcAcc[4]);
break;
case 0x52: memcpy(&stcGyro,&chrTemp[2],8);
w[0] = (short)((short)(stcGyro[1]) << 8 | stcGyro[0]);
w[1] = (short)((short)(stcGyro[3]) << 8 | stcGyro[2]);
w[2] = (short)((short)(stcGyro[5]) << 8 | stcGyro[4]);
break;
case 0x53: memcpy(&stcAngle,&chrTemp[2],8);
Angle[0] = (short)((short)(stcAngle[1]) << 8 | stcAngle[0]);
Angle[1] = (short)((short)(stcAngle[3]) << 8 | stcAngle[2]);
Angle[2] = (short)((short)(stcAngle[5]) << 8 | stcAngle[4]);
break;
//default: break;
}
RxLength -= 11;
memcpy(&chrTemp[0],&chrTemp[11],RxLength);
}
}
wait_us(100000);
if (Cnt++>=0)
{
Cnt=0;
ax=(float)a[0]/32768*16;
ay=(float)a[1]/32768*16;
az=(float)a[2]/32768*16;
gx=(float)w[0]/32768*2000;
gy=(float)w[1]/32768*2000;
gz=(float)w[2]/32768*2000;
roll=(float)Angle[0]/32768*180;
pitch=(float)Angle[1]/32768*180;
yaw=(float)Angle[2]/32768*180;
}
}
void gostraight(){
STBY=1;
PWMA.period(0.0001f);
PWMB.period(0.0001f);
PWMA.write(0.4f);
PWMB.write(0.413f);
AIN_1=0;
AIN_2=1;
BIN_1=0;
BIN_2=1;
}
void goback(){
STBY=1;
PWMA.period(0.0001f);
PWMB.period(0.0001f);
PWMA.write(0.30f);
PWMB.write(0.31f);
AIN_1=1;
AIN_2=0;
BIN_1=1;
BIN_2=0;
}
void turnleft(){
STBY=1;
PWMA.period(0.0001f);
PWMB.period(0.0001f);
PWMA.write(0.15f);
PWMB.write(0.15f);
AIN_1=1;
AIN_2=0;
BIN_1=0;
BIN_2=1;
}
void turnright(){
STBY=1;
PWMA.period(0.0001f);
PWMB.period(0.0001f);
PWMA.write(0.20f);
PWMB.write(0.20f);
AIN_1=0;
AIN_2=1;
BIN_1=1;
BIN_2=0;
}
void stop(){
STBY=0;
PWMA.period(0.0001f);
PWMB.period(0.0001f);
PWMA.write(0.15f);
PWMB.write(0.15f);
AIN_1=0;
AIN_2=1;
BIN_1=0;
BIN_2=1;
}
void gostraight_correct(){
if(distance3>distance4 && distance4>20){
//向左偏时，向右修正
PWMA.write(0.4f);
PWMB.write(0.35f);
}
if(distance3<distance4 && (distance3<20 || distance4<20)){
//向右偏时，向左修正
PWMA.write(0.32f);
PWMB.write(0.42f);
}
if ((d34)>=-1 && (d34)<=1 ){
//不偏时直走
PWMA.write(0.3f);
PWMB.write(0.3135f);
}
STBY=1;
PWMA.period(0.0001f);
PWMB.period(0.0001f);
AIN_1=0;
AIN_2=1;
BIN_1=0;
BIN_2=1;
wait_us(5e4);
printf("***** CORRECTING *****\n");
}
float initial_yaw=0;
float difference;
void trace_find(){
//执行到转向点的程序
gostraight();
wait_us(9.5e6);
stop();
wait_us(1e6);
goback();
wait_us(2e6);
stop();
wait_us(1e6);
//停在转向点，等待。
getJY61data();
printf("the yaw = %.1f\n", yaw);
printf("Flag = %d\n\n", flag);
//判断状态和读取到的AB数据。
//右箭头，01
if(A==0 && B==1){
turnleft();//转45°
while(1){
getJY61data();
printf("yaw during turning= %.3f\n", yaw);
if(initial_yaw<145){
difference=yaw-initial_yaw;
}else{
difference=yaw-initial_yaw+360;
}
if(difference>35){
stop();
printf("yaw after turning= %.3f\n", yaw);
wait_us(1e6);
break;
}
}
//转向AB=01的上线路。
gostraight();
wait_us(10.5e6);
stop();
wait_us(1e6);
goback();
wait_us(3e6);
stop();
wait_us(1e6);
//向右转，直走至栏杆边，结束后flag=1，进入阶段2.
turnright();
while(1){
getJY61data();
if((yaw-initial_yaw)<15){
stop();
wait_us(1e6);
flag=2;
break;
}
flag=2;
}
}
//上箭头，10
else if(A==1 && B==0){
turnleft();//转90°
while(1){
getJY61data();
printf("yaw during turning= %.3f\n", yaw);
if(initial_yaw<100){
difference=yaw-initial_yaw;
}else{
difference=yaw-initial_yaw+360;
}
if(difference>80){
stop();
printf("yaw after turning= %.3f\n", yaw);
wait_us(1e6);
break;
}
}
gostraight();
wait_us(15e6);
stop();
wait_us(1e6);
goback();
wait_us(1e6);
stop();
wait_us(1e6);
turnright();//转90°
while(1){
getJY61data();
if((yaw-initial_yaw)<15){
stop();
wait_us(1e6);
flag=2;
break;
}
}
}
//左箭头，11
else if(A==1 && B==1){
turnleft();//转135°
while(1){
getJY61data();
printf("yaw during turning= %.3f\n", yaw);
if(initial_yaw<55){
difference=yaw-initial_yaw;
}else{
difference=yaw-initial_yaw+360;
}
if(difference>125){
stop();
printf("yaw after turning= %.3f\n", yaw);
wait_us(1e6);
break;
}
}
gostraight();
wait_us(10.5e6);
stop();
wait_us(1e6);
goback();
wait_us(6e6);
stop();
wait_us(1e6);
turnright();
while(1){
getJY61data();
if((yaw-initial_yaw)<15){
stop();
wait_us(1e6);
flag=2;
break;
}
}
}
}
void ultrasonic_2(){
//这个函数是超声波patio2的封装函数，非必要勿改
distance1=HCSR04_1.getCm();
distance2=HCSR04_2.getCm();
distance3=HCSR04_3.getCm();
distance4=HCSR04_4.getCm();
d34=distance3-distance4;
data_print();
if(distance1>40 && distance2>40 && flag_u==1){
gostraight();
printf("^^^^^ GO STRAIGHT ^^^^^\n");
}
else if(distance1<40 && distance2<40 && flag_u ==1){
stop();
wait_us(1e6);
turnleft();
printf("<<<<< turning LEFT 1\n");
while(1){
distance3=HCSR04_3.getCm();
distance4=HCSR04_4.getCm();
d34=distance3-distance4;
//持续更新distance的数据
if(distance3<25 && distance4<25 && abs(d34)<1){
stop();
printf("===== STOP turning LEFT =====\n");
wait_us(1e6);
break;
}
}
flag_u = 2 ;
}
else if(distance1>60 && distance2>60 && distance3<60 && distance4<60 && flag_u == 2){
//进入修正前进状态
gostraight_correct();
//PID
}
else if(distance1>60 && distance2>60 && distance3>60 && distance4<30 && flag_u == 2){
gostraight();
printf("!!!!! ready for turning RIGHT !!!!!\n");
wait_us(0.3e6);//0.7
stop();
wait_us(1e6);
}
else if(distance1>60 && distance2>60 && distance3>60 && distance4>60 && flag_u == 2){
printf("turning RIGHT >>>>>\n");
turnright();
wait_us(1.9e6);//1.9e6 3&3.135
gostraight();
wait_us(2e6);
}
else if(distance3<80 && distance4<80 && distance1<30 && distance2<30 && flag_u == 2){
printf("<<<<< turning LEFT 2\n");
turnleft();
flag_s=flag_s + 1;
while(1){
distance1=HCSR04_1.getCm();
distance2=HCSR04_2.getCm();
distance3=HCSR04_3.getCm();
distance4=HCSR04_4.getCm();
d34=distance3-distance4;
if(distance1>60 && distance2>60 && abs(d34)<1){
stop();
wait_us(1.1e6);
break;
}
}
}
else if(flag_s == 2){//
printf("@@@@@ TRANSH CAN DETECTING @@@@@\n");
while(1){
data_print();
gostraight_correct();
if(distance1<10 && distance2<10){
stop();
wait_us(3e6);
break;
}
}
flag_u=3;
flag=3;
}
}
void ball_drop(){
printf("||||| BALL DROPPING |||||\n");
droptheball();
wait_us(2e6);
gostraight();
wait_us(2e6);
goback();
wait_us(1e6);
stop();
flag=4;
}
void communicating(){
//转向，陀螺仪参与
getJY61data();
//printf("yaw=%.3f",yaw);
float mid_yaw=yaw;
printf("middle yaw=%.3f",mid_yaw);
turnleft();//转90°
wait_us(3.2e6);
gostraight();
wait_us(40e6);
// while(1){
// printf("ready to stop");
// getJY61data();
// printf("yaw=%.3f",yaw);
// if(mid_yaw<100){//最后根据difference参数修改
// difference=yaw-mid_yaw;
// } else {
// difference=yaw-mid_yaw+360;
// }
// if(difference>80){//参数可调
// stop();
// wait_us(1e6);
// break;
// }
// }
// gostraight();
// wait_us(40e6);
communication();
printf("<<<<< COMMUNICATING >>>>>");
wait_us(5e6);
}
int main(){
wait_us(3e6);
COM = 0;
SER = 0;
//整个系统开始执行任务之前就读取初始角度，并赋值给initial_yaw。
getJY61data();
getJY61data();
initial_yaw=yaw;
while(1){
switch (flag){
case 1: {
trace_find();
flag = 2;
break;
}
case 2: {
while(1){
ultrasonic_2();
if(flag==3) {
break;
}
}
break;
}
case 3: {
ball_drop();
flag = 4;
break;
}
case 4: {
communicating();
break;
}
}
}
}2023-06-17 开始写 Patio 2 的超声波系统整合的内容
在 23.04.29.18.53 TDPS报告里面，有详细的思路和想法。
这里梳理一个超声波部分的单独的代码：
#include "mbed.h"
#include "HCSR04.h"
//pin set of motor
DigitalOut AIN_1(PA_9);
DigitalOut AIN_2(D9);
DigitalOut BIN_1(PB_5);
DigitalOut BIN_2(PA_10);
DigitalOut STBY(D10);
PwmOut PWMA(D7);
PwmOut PWMB(D6);
//pin set of Ultrasonic part
HCSR04 HCSR04_1(PB_12, PB_11);
HCSR04 HCSR04_2(PB_13, PB_14);
HCSR04 HCSR04_3(PC_11, PC_10);
HCSR04 HCSR04_4(PC_9, PC_8);
float distance1=0;
float distance2=0;
float distance3=0;
float distance4=0;
float d34=0;
float ax,ay,az,gx,gy,gz,roll,pitch,yaw;
volatile int flag=1;//system flag, to state which part it is
volatile int flag_u=1;//flag of ultrasonic to distinguish different function
volatile int flag_s=0;//flag for ultrasonic to distinguish current state
void gostraight(){
STBY=1;
PWMA.period(0.0001f);
PWMB.period(0.0001f);
PWMA.write(0.4f);
PWMB.write(0.413f);
AIN_1=0;
AIN_2=1;
BIN_1=0;
BIN_2=1;
}
void goback(){
STBY=1;
PWMA.period(0.0001f);
PWMB.period(0.0001f);
PWMA.write(0.30f);
PWMB.write(0.31f);
AIN_1=1;
AIN_2=0;
BIN_1=1;
BIN_2=0;
}
void turnleft(){
STBY=1;
PWMA.period(0.0001f);
PWMB.period(0.0001f);
PWMA.write(0.15f);
PWMB.write(0.15f);
AIN_1=1;
AIN_2=0;
BIN_1=0;
BIN_2=1;
}
void turnright(){
STBY=1;
PWMA.period(0.0001f);
PWMB.period(0.0001f);
PWMA.write(0.20f);
PWMB.write(0.20f);
AIN_1=0;
AIN_2=1;
BIN_1=1;
BIN_2=0;
}
void stop(){
STBY=0;
PWMA.period(0.0001f);
PWMB.period(0.0001f);
PWMA.write(0.15f);
PWMB.write(0.15f);
AIN_1=0;
AIN_2=1;
BIN_1=0;
BIN_2=1;
}
void gostraight_correct(){
if(distance3>distance4 && distance4>20){
//向左偏时，向右修正
PWMA.write(0.4f);
PWMB.write(0.35f);
}
if(distance3<distance4 && (distance3<20 || distance4<20)){
//向右偏时，向左修正
PWMA.write(0.32f);
PWMB.write(0.42f);
}
if ((d34)>=-1 && (d34)<=1 ){
//不偏时直走
PWMA.write(0.3f);
PWMB.write(0.3135f);
}
STBY=1;
PWMA.period(0.0001f);
PWMB.period(0.0001f);
AIN_1=0;
AIN_2=1;
BIN_1=0;
BIN_2=1;
wait_us(5e4);
printf("***** CORRECTING *****\n");
}
void ultrasonic_2(){
distance1=HCSR04_1.getCm();
distance2=HCSR04_2.getCm();
distance3=HCSR04_3.getCm();
distance4=HCSR04_4.getCm();
d34=distance3-distance4;
data_print();
if(distance1>40 && distance2>40 && flag_u==1){
gostraight();
printf("^^^^^ GO STRAIGHT ^^^^^\n");
}
else if(distance1<40 && distance2<40 && flag_u ==1){
stop();
wait_us(1e6);
turnleft();
printf("<<<<< turning LEFT 1\n");
while(1){
distance3=HCSR04_3.getCm();
distance4=HCSR04_4.getCm();
d34=distance3-distance4;
//持续更新distance的数据
if(distance3<25 && distance4<25 && abs(d34)<1){
stop();
printf("===== STOP turning LEFT =====\n");
wait_us(1e6);
break;
}
}
flag_u = 2 ;
}
else if(distance1>60 && distance2>60 && distance3<60 && distance4<60 && flag_u == 2){
//进入修正前进状态
gostraight_correct();
//PID
}
else if(distance1>60 && distance2>60 && distance3>60 && distance4<30 && flag_u == 2){
gostraight();
printf("!!!!! ready for turning RIGHT !!!!!\n");
wait_us(0.3e6);//0.7
stop();
wait_us(1e6);
}
else if(distance1>60 && distance2>60 && distance3>60 && distance4>60 && flag_u == 2){
printf("turning RIGHT >>>>>\n");
turnright();
wait_us(1.9e6);//1.9e6 3&3.135
gostraight();
wait_us(2e6);
}
else if(distance3<80 && distance4<80 && distance1<30 && distance2<30 && flag_u == 2){
printf("<<<<< turning LEFT 2\n");
turnleft();
flag_s=flag_s + 1;
while(1){
distance1=HCSR04_1.getCm();
distance2=HCSR04_2.getCm();
distance3=HCSR04_3.getCm();
distance4=HCSR04_4.getCm();
d34=distance3-distance4;
if(distance1>60 && distance2>60 && abs(d34)<1){
stop();
wait_us(1.1e6);
break;
}
}
}
else if(flag_s == 2){//
printf("@@@@@ TRANSH CAN DETECTING @@@@@\n");
while(1){
data_print();
gostraight_correct();
if(distance1<10 && distance2<10){
stop();
wait_us(3e6);
break;
}
}
flag_u=3;
flag=3;
}
}
int main(){
wait_us(3e6);
while(1){
switch (flag){
case 1: {
trace_find();
flag = 2;
break;
}
case 2: {
while(1){
ultrasonic_2();
if(flag==3) {
break;
}
}
break;
}
case 3: {
ball_drop();
flag = 4;
break;
}
case 4: {
communicating();
break;
}
}
}
}