添加文件

.gitignore

@@ -30,3 +30,7 @@
 *.exe
 *.out
 *.app
+
+# VSCode
+.vscode/*
+*/.vscode/*

TaiChi/TaiChi.ino

@@ -0,0 +1,15 @@
+#include "move.h" //轮胎运动库
+#include "sensor.h" //传感器库
+#include "servo.h" //舵机库
+
+
+void setup() 
+{
+
+}
+
+
+void loop() 
+{
+
+}

TaiChi/move.cpp

@@ -0,0 +1,189 @@
+#include <Arduino.h>
+
+#include "move.h"
+
+
+Move::Move()
+{
+    global_speed_rate = DEFAULT_GLOBAL_SPEED_RATE;
+    current_move = STOP;
+    current_speed_rate = 0;
+}
+
+
+Move::Move(int global_speed_rate)
+{
+    Move();
+    
+    this->global_speed_rate = global_speed_rate;
+}
+
+
+//设置全局速度比率
+void Move::SetGlobalSpeedRate(int global_speed_rate)
+{
+    this->global_speed_rate = global_speed_rate;
+}
+
+
+//获取当前运动方向
+uint8_t Move::GetCurrentMove(void)
+{
+    return current_move;
+}
+
+
+//获取当前运动速度比率
+int Move::GetCurrentSpeedRate(void)
+{
+    return current_speed_rate;
+}
+
+
+//控制某个轮子转动
+void Move::Wheel(uint8_t wheel, uint8_t rotation, int speed_rate)
+{
+    uint8_t pin_in1, pin_in2, pin_ena;
+
+    switch (wheel)
+    {
+    case LEFT_A_WHEEL:
+    {
+        pin_in1 = LEFT_L298N_IN1;
+        pin_in2 = LEFT_L298N_IN2;
+        pin_ena = LEFT_L298N_ENA;
+    } break;
+
+    case LEFT_B_WHEEL:
+    {
+        pin_in1 = LEFT_L298N_IN3;
+        pin_in2 = LEFT_L298N_IN4;
+        pin_ena = LEFT_L298N_ENB;
+    } break; 
+
+    case RIGHT_A_WHEEL:
+    {
+        pin_in1 = RIGHT_L298N_IN1;
+        pin_in2 = RIGHT_L298N_IN2;
+        pin_ena = RIGHT_L298N_ENA;
+    } break;
+
+    case RIGHT_B_WHEEL:
+    {
+        pin_in1 = RIGHT_L298N_IN3;
+        pin_in2 = RIGHT_L298N_IN4;
+        pin_ena = RIGHT_L298N_ENB;
+    }
+    }
+
+    if (rotation == STOP_ROTATION) //停止转动
+    {
+        digitalWrite(pin_in1, LOW);
+        digitalWrite(pin_in2, LOW);
+        return; //结束函数
+    }
+    
+    analogWrite(pin_ena, speed_rate * global_speed_rate * 255.0 / 10000.0); //设置 PWM 波，即转速
+
+    if (rotation == FORWARD_ROTATION) //向前转动
+    {
+        digitalWrite(pin_in1, LOW);
+        digitalWrite(pin_in2, HIGH);
+    }
+    else //向后转动
+    {
+        digitalWrite(pin_in1, HIGH);
+        digitalWrite(pin_in2, LOW);
+    }
+}
+
+
+//前进
+void Move::Forward(int speed_rate)
+{
+    Wheel(LEFT_A_WHEEL, FORWARD_ROTATION, speed_rate);
+    Wheel(LEFT_B_WHEEL, FORWARD_ROTATION, speed_rate);
+    Wheel(RIGHT_A_WHEEL, FORWARD_ROTATION, speed_rate);
+    Wheel(RIGHT_B_WHEEL, FORWARD_ROTATION, speed_rate);
+
+    current_move = FORWARD;
+    current_speed_rate = speed_rate * global_speed_rate / 100.0;
+}
+
+
+//后退
+void Move::Backward(int speed_rate)
+{
+    Wheel(LEFT_A_WHEEL, BACKWARD_ROTATION, speed_rate);
+    Wheel(LEFT_B_WHEEL, BACKWARD_ROTATION, speed_rate);
+    Wheel(RIGHT_A_WHEEL, BACKWARD_ROTATION, speed_rate);
+    Wheel(RIGHT_B_WHEEL, BACKWARD_ROTATION, speed_rate);
+
+    current_move = BACKWARD;    
+    current_speed_rate = speed_rate * global_speed_rate / 100.0;
+}
+
+
+//向前左转
+void Move::ForLeftward(int speed_rate)
+{
+    Wheel(LEFT_A_WHEEL, FORWARD_ROTATION, TRUN_SPEED_RATE * speed_rate / 100.0);
+    Wheel(LEFT_B_WHEEL, FORWARD_ROTATION, TRUN_SPEED_RATE * speed_rate / 100.0);
+    Wheel(RIGHT_A_WHEEL, FORWARD_ROTATION, speed_rate);
+    Wheel(RIGHT_B_WHEEL, FORWARD_ROTATION, speed_rate);
+
+    current_move = FORLEFTWARD;
+    current_speed_rate = speed_rate * global_speed_rate / 100.0;
+}
+
+
+//向前右转
+void Move::ForRightward(int speed_rate)
+{
+    Wheel(LEFT_A_WHEEL, FORWARD_ROTATION, speed_rate);
+    Wheel(LEFT_B_WHEEL, FORWARD_ROTATION, speed_rate);
+    Wheel(RIGHT_A_WHEEL, FORWARD_ROTATION, TRUN_SPEED_RATE * speed_rate / 100.0);
+    Wheel(RIGHT_B_WHEEL, FORWARD_ROTATION, TRUN_SPEED_RATE * speed_rate / 100.0);
+
+    current_move = FORRIGHTWARD;   
+    current_speed_rate = speed_rate * global_speed_rate / 100.0;
+}
+
+
+//向后左转
+void Move::BackLeftward(int speed_rate)
+{
+    Wheel(LEFT_A_WHEEL, BACKWARD_ROTATION, TRUN_SPEED_RATE * speed_rate / 100.0);
+    Wheel(LEFT_B_WHEEL, BACKWARD_ROTATION, TRUN_SPEED_RATE * speed_rate / 100.0);
+    Wheel(RIGHT_A_WHEEL, BACKWARD_ROTATION, speed_rate);
+    Wheel(RIGHT_B_WHEEL, BACKWARD_ROTATION, speed_rate);
+
+    current_move = BACKLEFTWARD;    
+    current_speed_rate = speed_rate * global_speed_rate / 100.0;
+}
+
+
+//向后右转
+void Move::BackRightward(int speed_rate)
+{
+    Wheel(LEFT_A_WHEEL, BACKWARD_ROTATION, speed_rate);
+    Wheel(LEFT_B_WHEEL, BACKWARD_ROTATION, speed_rate);
+    Wheel(RIGHT_A_WHEEL, BACKWARD_ROTATION, TRUN_SPEED_RATE * speed_rate / 100.0);
+    Wheel(RIGHT_B_WHEEL, BACKWARD_ROTATION, TRUN_SPEED_RATE * speed_rate / 100.0);
+
+    current_move = BACKRIGHTWARD;     
+    current_speed_rate = speed_rate * global_speed_rate / 100.0;    
+}
+
+
+//制动
+void Move::Stop(void)
+{
+    Wheel(LEFT_A_WHEEL, STOP_ROTATION);
+    Wheel(LEFT_B_WHEEL, STOP_ROTATION);
+    Wheel(RIGHT_A_WHEEL, STOP_ROTATION);
+    Wheel(RIGHT_B_WHEEL, STOP_ROTATION);  
+
+    current_move = STOP;
+    current_speed_rate = 0;
+}

TaiChi/move.h

@@ -0,0 +1,75 @@
+#ifndef MOVE_H
+#define MOVE_H
+
+//轮胎定义
+#define LEFT_A_WHEEL 0
+#define LEFT_B_WHEEL 1
+#define RIGHT_A_WHEEL 2
+#define RIGHT_B_WHEEL 3
+
+//轮胎旋转方向定义，包括停止
+#define FORWARD_ROTATION 0
+#define BACKWARD_ROTATION 1
+#define STOP_ROTATION 3
+
+//运动状态定义
+#define FORWARD 0
+#define BACKWARD 1
+#define FORLEFTWARD 2
+#define BACKLEFTWARD 3
+#define FORRIGHTWARD 4
+#define BACKRIGHTWARD 5
+#define STOP 6
+
+//默认全局速度比率
+#define DEFAULT_GLOBAL_SPEED_RATE 100
+
+//转向时一侧减速的比率
+#define TRUN_SPEED_RATE 50
+
+//左侧 L298N 接口定义
+#define LEFT_L298N_IN1 22
+#define LEFT_L298N_IN2 23
+#define LEFT_L298N_IN3 24
+#define LEFT_L298N_IN4 25
+#define LEFT_L298N_ENA 8
+#define LEFT_L298N_ENB 9
+
+//右侧 L298N 接口定义
+#define RIGHT_L298N_IN1 26
+#define RIGHT_L298N_IN2 27
+#define RIGHT_L298N_IN3 28
+#define RIGHT_L298N_IN4 29
+#define RIGHT_L298N_ENA 10
+#define RIGHT_L298N_ENB 11
+
+
+class Move
+{
+public:
+    Move();
+    Move(int global_speed_rate);
+
+    void SetGlobalSpeedRate(int global_speed_rate); //设置全局速度比率
+
+    uint8_t GetCurrentMove(void); //获取当前运动方向
+    int GetCurrentSpeedRate(void); //获取当前运动速度比率
+
+    void Wheel(uint8_t wheel, uint8_t rotation, int speed_rate = 100); //控制某个轮子转动
+
+    void Forward(int speed_rate = 100); //前进
+    void Backward(int speed_rate = 100); //后退
+    void ForLeftward(int speed_rate = 100); //向前左转
+    void ForRightward(int speed_rate = 100); //向前右转
+    void BackLeftward(int speed_rate = 100); //向后左转
+    void BackRightward(int speed_rate = 100); //向后右转
+    void Stop(void); //制动
+
+private:
+    int global_speed_rate; //全局速度比率
+    uint8_t current_move; //当前运动状态
+    int current_speed_rate; //当前运动速度比率
+};
+
+
+#endif

TaiChi/sensor.cpp

@@ -0,0 +1,76 @@
+#include <Arduino.h>
+
+#include "sensor.h"
+
+
+Sensor::Sensor()
+{
+    pinMode(BUTTON_1_OUT, INPUT);
+    pinMode(BUTTON_2_OUT, INPUT);
+}
+
+
+bool Sensor::IsWhite(uint8_t gray_sensor_num)
+{
+    uint8_t gray_out_pin;
+    int gray_val;
+    
+    switch (gray_sensor_num)
+    {
+    case GRAY_1: gray_out_pin = GRAY_1_OUT; break;
+    case GRAY_2: gray_out_pin = GRAY_2_OUT; break;
+    case GRAY_3: gray_out_pin = GRAY_3_OUT; break;
+    case GRAY_4: gray_out_pin = GRAY_4_OUT; break;
+    case GRAY_5: gray_out_pin = GRAY_5_OUT; break;
+    case GRAY_6: gray_out_pin = GRAY_6_OUT;
+    }
+
+    gray_val = analogRead(gray_out_pin);
+
+    #ifdef DEBUG
+    //调试输出灰度值
+    switch (gray_sensor_num)
+    {
+    case GRAY_1: Serial.print("GRAY_1: "); break;
+    case GRAY_2: Serial.print("GRAY_2: "); break;
+    case GRAY_3: Serial.print("GRAY_3: "); break;
+    case GRAY_4: Serial.print("GRAY_4: "); break;
+    case GRAY_5: Serial.print("GRAY_5: "); break;
+    case GRAY_6: Serial.print("GRAY_6: ");
+    }
+
+    Serial.println(gray_val);
+    #endif
+
+    if (gray_val > GRAY_GATE_VAL)
+        return true;
+    else return false;
+}
+
+
+bool Sensor::IsPushed(uint8_t button_num)
+{
+    uint8_t button_out_pin;
+    int button_val;
+
+    if (button_num == BUTTON_1)
+        button_out_pin = BUTTON_1_OUT;
+    else button_out_pin = BUTTON_2_OUT;
+
+    button_val = digitalRead(button_out_pin);
+
+    #ifdef DEBUG
+    //调试输出按钮状态
+    if (button_num == BUTTON_1)
+        Serial.print("BUTTON_1: ");
+    else Serial.print("BUTTON_2: ");
+
+    if (button_val == LOW)
+        Serial.print("pushed\n");
+    else Serial.print("released\n");
+    #endif    
+
+    if (button_val == LOW)
+        return true;
+    else return false;
+}

TaiChi/sensor.h

@@ -0,0 +1,45 @@
+#ifndef SENSOR_H
+#define SENSOR_H
+
+#define DEBUG
+
+//灰度传感器接口定义
+#define GRAY_1_OUT A0
+#define GRAY_2_OUT A1
+#define GRAY_3_OUT A2
+#define GRAY_4_OUT A3
+#define GRAY_5_OUT A4
+#define GRAY_6_OUT A5
+
+//灰度传感器临界值
+#define GRAY_GATE_VAL 800
+
+//灰度传感器标识定义
+#define GRAY_1 0
+#define GRAY_2 1
+#define GRAY_3 2
+#define GRAY_4 3
+#define GRAY_5 4
+#define GRAY_6 5
+
+//碰撞传感器接口定义
+#define BUTTON_1_OUT 2
+#define BUTTON_2_OUT 3
+
+//碰撞传感器标识定义
+#define BUTTON_1 0
+#define BUTTON_2 1
+
+
+class Sensor
+{
+public:
+    Sensor();
+
+    bool IsWhite(uint8_t gray_sensor_num);
+
+    bool IsPushed(uint8_t button_num);
+};
+
+
+#endif

TaiChi/servo.cpp

@@ -0,0 +1,134 @@
+#include "servo.h"
+
+
+#define GET_LOW_BYTE(A) (uint8_t)((A))
+//宏函数 获得A的低八位
+#define GET_HIGH_BYTE(A) (uint8_t)((A) >> 8)
+//宏函数 获得A的高八位
+#define BYTE_TO_HW(A, B) ((((uint16_t)(A)) << 8) | (uint8_t)(B))
+//宏函数 以A为高八位 B为低八位 合并为16位整形
+
+
+Servo::Servo()
+{
+    SerialX = &Serial1; //默认使用 Mega 板 18 19 作为串口通信端口
+    SerialX->begin(BAUD_RATE);
+}
+
+
+//指定串口通信端口
+Servo::Servo(HardwareSerial& serial_num)
+{
+    SerialX = &serial_num;
+    SerialX->begin(BAUD_RATE);
+}
+
+
+/*********************************************************************************
+ * Function:  RunActionGroup
+ * Description： 运行指定动作组
+ * Parameters:   action_num:动作组序号, times:执行次数
+ * Return:       无返回
+ * Others:       times = 0 时无限循环
+ *********************************************************************************/
+void Servo::RunActionGroup(uint8_t action_num, uint16_t times)
+{
+    uint8_t buf[7];
+    buf[0] = FRAME_HEADER; //填充帧头
+    buf[1] = FRAME_HEADER;
+    buf[2] = 5;                    //数据长度，数据帧除帧头部分数据字节数，此命令固定为5
+    buf[3] = CMD_ACTION_GROUP_RUN; //填充运行动作组命令
+    buf[4] = action_num;          //填充要运行的动作组号
+    buf[5] = GET_LOW_BYTE(times);  //取得要运行次数的低八位
+    buf[6] = GET_HIGH_BYTE(times); //取得要运行次数的高八位
+    SerialX->write(buf, 7);        //发送数据帧
+}
+
+
+/*********************************************************************************
+ * Function:  StopActionGroup
+ * Description： 停止动作组运行
+ * Parameters:   speed: 目标速度
+ * Return:       无返回
+ * Others:
+ *********************************************************************************/
+void Servo::StopActionGroup(void)
+{
+    uint8_t buf[4];
+    buf[0] = FRAME_HEADER; //填充帧头
+    buf[1] = FRAME_HEADER;
+    buf[2] = 2;                     //数据长度，数据帧除帧头部分数据字节数，此命令固定为2
+    buf[3] = CMD_ACTION_GROUP_STOP; //填充停止运行动作组命令
+
+    SerialX->write(buf, 4); //发送数据帧
+}
+
+
+/*********************************************************************************
+ * Function:  SetActionGroupSpeed
+ * Description： 设定指定动作组的运行速度
+ * Parameters:   action_num: 动作组序号 , speed:目标速度
+ * Return:       无返回
+ * Others:
+ *********************************************************************************/
+void Servo::SetActionGroupSpeed(uint8_t action_num, uint16_t speed)
+{
+    uint8_t buf[7];
+    buf[0] = FRAME_HEADER; //填充帧头
+    buf[1] = FRAME_HEADER;
+    buf[2] = 5;                      //数据长度，数据帧除帧头部分数据字节数，此命令固定为5
+    buf[3] = CMD_ACTION_GROUP_SPEED; //填充设置动作组速度命令
+    buf[4] = action_num;            //填充要设置的动作组号
+    buf[5] = GET_LOW_BYTE(speed);    //获得目标速度的低八位
+    buf[6] = GET_HIGH_BYTE(speed);   //获得目标熟读的高八位
+
+    SerialX->write(buf, 7); //发送数据帧
+}
+
+
+/*********************************************************************************
+ * Function:  SetAllActionGroupSpeed
+ * Description： 设置所有动作组的运行速度
+ * Parameters:   speed: 目标速度
+ * Return:       无返回
+ * Others:
+ *********************************************************************************/
+void Servo::SetAllActionGroupSpeed(uint16_t speed)
+{
+    SetActionGroupSpeed(0xFF, speed); //调用动作组速度设定，组号为0xFF时设置所有组的速度
+}
+
+
+//恢复初始状态，指定速度
+void Servo::Reset(uint16_t speed)
+{
+    SetActionGroupSpeed(ACTION_RESET_NUM, speed);
+    RunActionGroup(ACTION_RESET_NUM, 1);
+    SetActionGroupSpeed(ACTION_RESET_NUM, SERVO_NORMAL_SPEED);
+}
+
+
+//抓取，指定速度
+void Servo::Catch(uint16_t speed)
+{
+    SetActionGroupSpeed(ACTION_CATCH_NUM, speed);
+    RunActionGroup(ACTION_CATCH_NUM, 1);
+    SetActionGroupSpeed(ACTION_CATCH_NUM, SERVO_NORMAL_SPEED);
+}
+
+
+//释放，指定速度
+void Servo::Release(uint16_t speed)
+{
+    SetActionGroupSpeed(ACTION_RELEASE_NUM, speed);
+    RunActionGroup(ACTION_RELEASE_NUM, 1);
+    SetActionGroupSpeed(ACTION_RELEASE_NUM, SERVO_NORMAL_SPEED);
+}
+
+
+//停止舵机并恢复初始状态，指定速度
+void Servo::StopAndReset(uint16_t speed)
+{
+    StopActionGroup();
+    Reset(speed);
+}

TaiChi/servo.h

@@ -0,0 +1,49 @@
+#ifndef SERVO_H
+#define SERVO_H
+
+
+#include <Arduino.h>
+
+
+#define BAUD_RATE 9600
+
+
+//发送部分的指令
+#define FRAME_HEADER 0x55            //帧头
+#define CMD_ACTION_GROUP_RUN 0x06    //运行动作组指令
+#define CMD_ACTION_GROUP_STOP 0x07   //停止动作组运行指令
+#define CMD_ACTION_GROUP_SPEED 0x0B  //设置动作组运行速度指令
+
+
+//动作组
+#define ACTION_RESET_NUM 99
+#define ACTION_CATCH_NUM 100
+#define ACTION_RELEASE_NUM 101
+
+
+//默认动作组速度
+#define SERVO_NORMAL_SPEED 100
+
+
+class Servo
+{
+public:
+    Servo();
+    Servo(HardwareSerial &serial_num);
+
+    void RunActionGroup(uint8_t action_num, uint16_t times);
+    void StopActionGroup(void);
+    void SetActionGroupSpeed(uint8_t action_num, uint16_t speed);
+    void SetAllActionGroupSpeed(uint16_t speed);
+
+    void Reset(uint16_t speed = SERVO_NORMAL_SPEED); //恢复初始状态，指定速度
+    void Catch(uint16_t speed = SERVO_NORMAL_SPEED); //抓取，指定速度
+    void Release(uint16_t speed = SERVO_NORMAL_SPEED); //释放，指定速度
+    void StopAndReset(uint16_t speed = SERVO_NORMAL_SPEED); //停止舵机并恢复初始状态，指定速度
+
+private:
+    HardwareSerial *SerialX;
+};
+
+
+#endif