ROV控制系统程序代码解析

#include <MsTimer2.h> #include <LedControl.h> #include <Servo.h> Servo Left_Front_Thruster;//左前推进器对象 Servo Left_Upper_Thruster;//左上推进器对象 Servo Right_Front_Thruster;//右前推进器对象 Servo Right_Upper_Thruster;//右上推进器对象 Servo Arm;//机械臂对象

#define LEFT_FRONT_PIN 3//左前推进器引脚 #define LEFT_UPPER_PIN 5//左上推进器引脚 #define RIGHT_FRONT_PIN 9//右前推进器引脚 #define RIGHT_UPPER_PIN 6//右上推进器引脚 #define ARM_PIN 10//机械臂引脚

struct { uint8_t Act_Value[9] = {0, 0, 0, 0, 0, 0, 0, 0, 0}; //动作幅值 前进后退左转右转上浮下潜左翻右翻 0-110 舵机 0-140 long int Ctl_Value[5] = {1500, 1500, 1500, 1500, 0}; //控制量 左前右前左上右上 机械臂 long int Ctl_ValueLast[5]; //上一次控制量 long int count;//超时关机计数器 } ROV_Ctl;

/LED点阵变量定义区/ #define DIN 2 #define CS 4 #define CLK 7 LedControl lc = LedControl(DIN, CLK, CS, 4); byte Zero[8] = {0x3C, 0x66, 0x66, 0x66, 0x66, 0x66, 0x66, 0x3C};

/串口通信变量定义区/ #define Buffer_Lenth 9 //接收数据数组个数 long int Receive_Data_Ture = 0; //数据正确标志位 unsigned char Serial_Receive_Data[Buffer_Lenth]; //串口接收数据缓存区 char Communicate_state = 0; //通信状态标志位

///手柄结构体定义区/ //ruct //{ // /摇杆值/ // uint8_t Joystick_Value[4]; //0:右摇杆前后运动数值1:右摇杆左右运动数值2:左摇杆前后运动数值3:左摇杆左右运动数值 // /按键值/ // uint8_t Key_Value; //按键值依次:▲■×↑↓←→L1L2R1R2 //} Ctl_Handle;

void setup() { Serial.begin(9600);

pinMode(A0, OUTPUT); Left_Front_Thruster.attach(LEFT_FRONT_PIN); Left_Upper_Thruster.attach(LEFT_UPPER_PIN); Right_Front_Thruster.attach(RIGHT_FRONT_PIN); Right_Upper_Thruster.attach(RIGHT_UPPER_PIN); Arm.attach(ARM_PIN);

Left_Front_Thruster.writeMicroseconds(1000); Left_Upper_Thruster.writeMicroseconds(1000); Right_Front_Thruster.writeMicroseconds(1000); Right_Upper_Thruster.writeMicroseconds(1000); delay(10); Left_Front_Thruster.writeMicroseconds(1500); Left_Upper_Thruster.writeMicroseconds(1500); Right_Front_Thruster.writeMicroseconds(1500); Right_Upper_Thruster.writeMicroseconds(1500);

led(); MsTimer2::set(500, shut); MsTimer2::start(); }

void loop() { }

void shut() { Receive_Data_Ture--; if (Receive_Data_Ture > 0) { ROV_Ctl.count = 0; } if (Receive_Data_Ture <= 0) { ROV_Ctl.count++; } Serial.println(ROV_Ctl.count); if (ROV_Ctl.count > 100) { analogWrite(A0, 255); } }

uint8_t Num_len; #define USART1_REC_LEN 24 //定义最大接收字节数 200 实际69 unsigned char USART1_RX_BUF[USART1_REC_LEN]; //接收缓冲,最大USART_REC_LEN个字节.

void serialEvent() { /接收定长数据/ /EB 90 00 01 02 03 04 05 06 07/ /获取缓存区缓存数据个数/ uint8_t Para; int Rx_Length = Serial.available(); if (Rx_Length > 0) { Para = Serial.read(); if (Para == 0xEB) { Num_len = 0; } USART1_RX_BUF[Num_len++] = Para; //将接受到的数据存入RX_BUFF if (Num_len == 9 && USART1_RX_BUF[0] == 0xEB && USART1_RX_BUF[1] == 0x90 && USART1_RX_BUF[2] == 0xA5) { memcpy(Serial_Receive_Data, USART1_RX_BUF, Num_len); //保存数据 Num_len = 0; memset(USART1_RX_BUF, 0, USART1_REC_LEN); //清空BUF Receive_Data_Ture = 2; handle(); mapping(); execute(); } } }

void handle() {

}

void mapping() {

}

void execute() {

}

void led() { lc.shutdown(0, false); //启动时，MAX72XX处于省电模式 lc.setIntensity(0, 8); //将亮度设置为最大值 lc.clearDisplay(0); for (int i = 0; i < 8; i++) { lc.setRow(0, i, Zero[i]); } }

// 按模块进行解释内容：注释已经给出了各个模块的作用，这里再进行补充：

// 1. 电机控制模块： // 通过Servo库控制四个舵机电机以及机械臂，实现ROV的运动和抓取动作。

// 2. LED点阵模块： // 通过LedControl库控制LED点阵，用于显示ROV状态和提示信息。

// 3. 串口通信模块： // 通过Serial库进行串口通信，接收上位机发送的控制命令。

// 4. 超时关机模块： // 通过MsTimer2库实现定时器功能，实现ROV在长时间未接收到控制命令时自动关机的功能。

// 5. 控制量映射模块： // 根据接收到的控制命令，将其映射为对应的控制量，用于电机控制。

// 6. 控制执行模块： // 根据映射后的控制量，执行对应的控制动作，实现ROV的运动和抓取动作。