Arduino 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() { uint8_t R1 = Serial_Receive_Data[3]; //右摇杆前后运动数值 uint8_t R2 = Serial_Receive_Data[4]; //右摇杆左右运动数值 uint8_t L1 = Serial_Receive_Data[5]; //左摇杆前后运动数值 uint8_t L2 = Serial_Receive_Data[6]; //左摇杆左右运动数值 uint8_t key = Serial_Receive_Data[7]; //按键值

// 将摇杆值映射到舵机控制量 ROV_Ctl.Ctl_Value[0] = map(R1, 0, 255, 1000, 2000); ROV_Ctl.Ctl_Value[1] = map(R2, 0, 255, 1000, 2000); ROV_Ctl.Ctl_Value[2] = map(L1, 0, 255, 1000, 2000); ROV_Ctl.Ctl_Value[3] = map(L2, 0, 255, 1000, 2000);

// 将按键值映射到机械臂控制量 if ((key & 0x01) == 0x01) // ▲键 { ROV_Ctl.Ctl_Value[4] += 10; } if ((key & 0x02) == 0x02) // ■键 { ROV_Ctl.Ctl_Value[4] -= 10; } }

void execute() { // 控制舵机 Left_Front_Thruster.writeMicroseconds(ROV_Ctl.Ctl_Value[0]); Right_Front_Thruster.writeMicroseconds(ROV_Ctl.Ctl_Value[1]); Left_Upper_Thruster.writeMicroseconds(ROV_Ctl.Ctl_Value[2]); Right_Upper_Thruster.writeMicroseconds(ROV_Ctl.Ctl_Value[3]);

// 控制机械臂 Arm.write(ROV_Ctl.Ctl_Value[4]);

// 更新上一次控制量 for (int i = 0; i < 5; i++) { ROV_Ctl.Ctl_ValueLast[i] = ROV_Ctl.Ctl_Value[i]; } }

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]); } }

改变通信协议为 Serial.write(0xEB); Serial.write(0x90); Serial.write(R1int); Serial.write(R2int); Serial.write(L1int); Serial.write(L2int); Serial.write(keyint); Serial.write(SUM);