STM32Cubemx-H7-17-麥克納姆輪驅動

前言? ?--末尾右總體的.c和.h

本篇文章把麥克納姆輪的代碼封裝到.c和.h，使用者只需要根據輪子正轉的方向，在.h處修改定義方向引腳，把輪子都統一正向后，后面的輪子驅動就可以正常了，然后直接調用函數驅動即可。

設置滿轉預充裝載值是1000

其中會有幾種方案

第一種是直接開環，沒有任何返款

第二種是編碼行走，開環走到指定位置，容易收重力導致走歪

第三種是陀螺儀行走，有陀螺儀閉環防止走歪

第四種是編碼陀螺儀，是第二種的升級版，走到指定距離且不歪

頭文件處的定義內容講解

我們先看.h，知道哪些函數會有哪些功能和宏定義參數

1.方向引腳，定時器引腳，編碼中斷引腳定義

首先?是在.h的宏定義處定義引腳編號，比如tb6612控制方向的引腳，以及設定了哪個定時器作為pwm輸出，還有編碼計次的中斷引腳

/* 硬件相關宏定義 *//*
1      前進方向往上       42                        3
*/
// TB6612 方向引腳定義
#define MOTOR1_PIN_A_PORT GPIOA
#define MOTOR1_PIN_A_PIN (1 << 8)
#define MOTOR1_PIN_B_PORT GPIOC
#define MOTOR1_PIN_B_PIN (1 << 9)#define MOTOR2_PIN_A_PORT GPIOC
#define MOTOR2_PIN_A_PIN (1 << 8)
#define MOTOR2_PIN_B_PORT GPIOD
#define MOTOR2_PIN_B_PIN (1 << 13)#define MOTOR3_PIN_A_PORT GPIOD
#define MOTOR3_PIN_A_PIN (1 << 9)
#define MOTOR3_PIN_B_PORT GPIOD
#define MOTOR3_PIN_B_PIN (1 << 10)#define MOTOR4_PIN_A_PORT GPIOD
#define MOTOR4_PIN_A_PIN (1 << 11)
#define MOTOR4_PIN_B_PORT GPIOD
#define MOTOR4_PIN_B_PIN (1 << 12)/* 定時器相關宏定義 */
#define PWM_TIMER &htim1
#define PWM_CHANNEL_1 TIM_CHANNEL_1
#define PWM_CHANNEL_2 TIM_CHANNEL_2
#define PWM_CHANNEL_3 TIM_CHANNEL_3
#define PWM_CHANNEL_4 TIM_CHANNEL_4/* 編碼器相關宏定義 */
#define ENCODER_CHA_1_PIN GPIO_PIN_4
#define ENCODER_CHA_1_PORT GPIOA
#define ENCODER_CHA_2_PIN GPIO_PIN_2
#define ENCODER_CHA_2_PORT GPIOB
#define ENCODER_CHA_3_PIN GPIO_PIN_6
#define ENCODER_CHA_3_PORT GPIOE
#define ENCODER_CHA_4_PIN GPIO_PIN_12
#define ENCODER_CHA_4_PORT GPIOE

把自己的引腳根據原理圖填對應，方向引腳也是，如果發現反轉就把對應的引腳定義反過來，編碼也是，編碼我們用一個引腳就好，都在cubemx配置好。

2.參數定義

/* 控制參數相關宏定義 */
// 速度控制參數
#define MIN_ROTATION_DUTY_CYCLE 100
#define ROTATION_SPEED 300// PD控制器參數
#define BACK_KP 10        // 后退PD參數
#define BACK_KD 0
#define STRAIGHT_KP 10    // 直行PD參數
#define STRAIGHT_KD 0
#define LEFT_KP 30        // 橫向左轉PD參數
#define LEFT_KD 0
#define RIGHT_KP 30       // 橫向右轉PD參數
#define RIGHT_KD 0
#define ROTATE_KP 5       // 旋轉PD參數
#define ROTATE_KD 0.2// 陀螺儀配置
#define GYRO_POLARITY 0   /* 類型定義 */
typedef struct {float kp;            // 比例系數float kd;            // 微分系數float prev_error;    // 上一次誤差
} PDController;

這里的話包括，第一個是自旋轉的時候，最小的旋轉占空比，還有正常旋轉占空比。

然后是配合上陀螺儀時候的PID各個功能參數

還有一個是陀螺儀極性，調用函數后發現是往相反方向移動，那么就修改這個值，因為陀螺儀正放和倒放的極性不同，0和1代表不同的極性。可以觀察使用測試程序后，如果發現旋轉后離目標點越來越大，那就修改極性。

一? 功能函數的定義

后面會用到這么多函數，其中包括

1.基本開環函數

這里的函數就是不帶反饋的，給占空比值就動，會容易收到重力分布不均影響，導致走歪。

2.距離開環函數

這里的函數根據給定要跑的距離，跑到后停止，距離由uint16_t wheel_get_count(void);函數返回，但是跑的過程中可能會因為重力分布不均影響，導致走歪。

3.帶陀螺儀的閉環函數

這里的函數通過傳入標準值，然后方向會以標準值作為參考調制輪子占空比，從而防止走偏。不過使用的時候要在循環里面使用，因為只調用一次肯定是行不通的。

4.帶編碼的陀螺儀閉環函數

這個根據陀螺儀的值防止走偏，然后由根據編碼值確定要走的距離，最為精準。

/* 函數聲明 */
// 初始化函數
void wheel_init(void);
void wheel_begin_go(void);// 基礎控制函數
void wheel_stop(void);
void set_all_pwm_channels(uint16_t speed);
void set_all_pwm_channels_separate(uint16_t speed1, uint16_t speed2, uint16_t speed3, uint16_t speed4);
void tb6612_set_direction(uint8_t motor_num, uint8_t polarity);// 閉環控制函數
void wheel_go_back(uint16_t now_z, uint16_t speed);
// 修改函數名
void wheel_go_forward(uint16_t now_z, uint16_t speed);
void wheel_go_right(uint16_t now_z, uint16_t speed);
void wheel_go_left(uint16_t now_z, uint16_t speed);
void wheel_rotate(int16_t angle);// 開環控制函數
void wheel_go_forward_openloop(uint16_t speed);
void wheel_go_back_openloop(uint16_t speed);
void wheel_go_left_openloop(uint16_t speed);
void wheel_go_right_openloop(uint16_t speed);
void wheel_rotate_left_openloop(uint16_t speed);
void wheel_rotate_right_openloop(uint16_t speed);// 帶編碼器的閉環控制函數
// 修改函數名
void wheel_go_forward_count_close(uint16_t count, uint16_t speed);
void wheel_go_back_count_close(uint16_t count, uint16_t speed);
void wheel_go_left_count_close(uint16_t count, uint16_t speed);
void wheel_go_right_count_close(uint16_t count, uint16_t speed);// 帶編碼器的開環控制函數
void wheel_go_forward_count_open(uint16_t count, uint16_t speed);
void wheel_go_back_count_open(uint16_t count, uint16_t speed);
void wheel_go_left_count_open(uint16_t count, uint16_t speed);
void wheel_go_right_count_open(uint16_t count, uint16_t speed);// 輔助函數
uint16_t clamp_speed(uint16_t speed, uint16_t error);
uint16_t get_angle_value(void);
uint16_t wheel_get_count(void);
int32_t count_save(uint8_t cmd);
void wheel_polarity_test(void);#endif // WHEEL_H

二.輔助函數的介紹

有一些算法需要功能函數

1.過沖函數

? ? ? ?我們使用陀螺儀配合的時候，萬一根據差值相減后給的占空比小于0，因為是無符號整型，此時是無窮大的，這時候的占空比會拉滿，導致過沖現象。

/*** @brief 確保計算后的速度不小于0* @param speed 當前速度值* @param error 誤差值* @return 調整后的速度值*/
uint16_t clamp_speed(uint16_t speed, uint16_t error) {if (speed > error) {return speed - error;}return 0;
}/*** @brief 限制速度在合理范圍* @param speed 當前速度值* @param max_speed 最大速度限制* @return 調整后的速度值*/
uint16_t limit_speed(uint16_t speed, uint16_t max_speed) {if (speed > max_speed) {return max_speed;}return speed;
}

2.? 設置占空比函數

兩個函數，一個設置全部占空比，一個分別設置占空比

/*** @brief 設置四個通道PWM占空比(相同值)* @param speed PWM占空比值*/
void set_all_pwm_channels(uint16_t speed) {__HAL_TIM_SetCompare(PWM_TIMER, PWM_CHANNEL_1, speed);__HAL_TIM_SetCompare(PWM_TIMER, PWM_CHANNEL_2, speed);__HAL_TIM_SetCompare(PWM_TIMER, PWM_CHANNEL_3, speed);__HAL_TIM_SetCompare(PWM_TIMER, PWM_CHANNEL_4, speed);
}/*** @brief 設置四個通道PWM占空比(不同值)* @param speed1 通道1PWM值* @param speed2 通道2PWM值* @param speed3 通道3PWM值* @param speed4 通道4PWM值*/
void set_all_pwm_channels_separate(uint16_t speed1, uint16_t speed2, uint16_t speed3, uint16_t speed4) {__HAL_TIM_SetCompare(PWM_TIMER, PWM_CHANNEL_1, speed1);__HAL_TIM_SetCompare(PWM_TIMER, PWM_CHANNEL_2, speed2);__HAL_TIM_SetCompare(PWM_TIMER, PWM_CHANNEL_3, speed3);__HAL_TIM_SetCompare(PWM_TIMER, PWM_CHANNEL_4, speed4);
}

3.TB6612方向確認及初始化函數

這里主要是讓引腳定義規范，基本上默認1就是正轉

然后還有初始化和停車的函數

/*** @brief 控制TB6612電機方向* @param motor_num 電機編號(1-4)* @param polarity 方向(0或1)*/
void tb6612_set_direction(uint8_t motor_num, uint8_t polarity) {switch (motor_num) {case 1:if (polarity == 0) {HAL_GPIO_WritePin(MOTOR1_PIN_A_PORT, MOTOR1_PIN_A_PIN, 1);HAL_GPIO_WritePin(MOTOR1_PIN_B_PORT, MOTOR1_PIN_B_PIN, 0);} else {HAL_GPIO_WritePin(MOTOR1_PIN_A_PORT, MOTOR1_PIN_A_PIN, 0);HAL_GPIO_WritePin(MOTOR1_PIN_B_PORT, MOTOR1_PIN_B_PIN, 1);}break;case 2:if (polarity == 0) {HAL_GPIO_WritePin(MOTOR2_PIN_A_PORT, MOTOR2_PIN_A_PIN, 1);HAL_GPIO_WritePin(MOTOR2_PIN_B_PORT, MOTOR2_PIN_B_PIN, 0);} else {HAL_GPIO_WritePin(MOTOR2_PIN_A_PORT, MOTOR2_PIN_A_PIN, 0);HAL_GPIO_WritePin(MOTOR2_PIN_B_PORT, MOTOR2_PIN_B_PIN, 1);}break;case 3:if (polarity == 0) {HAL_GPIO_WritePin(MOTOR3_PIN_A_PORT, MOTOR3_PIN_A_PIN, 1);HAL_GPIO_WritePin(MOTOR3_PIN_B_PORT, MOTOR3_PIN_B_PIN, 0);} else {HAL_GPIO_WritePin(MOTOR3_PIN_A_PORT, MOTOR3_PIN_A_PIN, 0);HAL_GPIO_WritePin(MOTOR3_PIN_B_PORT, MOTOR3_PIN_B_PIN, 1);}break;case 4:if (polarity == 0) {HAL_GPIO_WritePin(MOTOR4_PIN_A_PORT, MOTOR4_PIN_A_PIN, 1);HAL_GPIO_WritePin(MOTOR4_PIN_B_PORT, MOTOR4_PIN_B_PIN, 0);} else {HAL_GPIO_WritePin(MOTOR4_PIN_A_PORT, MOTOR4_PIN_A_PIN, 0);HAL_GPIO_WritePin(MOTOR4_PIN_B_PORT, MOTOR4_PIN_B_PIN, 1);}break;default:break;}
}/*** @brief 初始化輪子控制模塊*/void wheel_init() {HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4); set_all_pwm_channels(0);}/*** @brief 停止所有輪子*/void wheel_stop() {set_all_pwm_channels(0);}

4.編碼計次及陀螺儀函數

每次啟動前可以用wheel_begin_go()函數清零看，走的過程中可以用?wheel_get_count()返回數值，get_angle_value()函數里，修改成你返回z軸角度值的函數?，修改成這樣調用方便，要不然定義的函數不一樣要修改的太多了。????????


/*** @brief 開始運動，重置編碼器計數*/
void wheel_begin_go()
{count_save(8);
}/*** @brief 獲取編碼器平均計數* @return 四個編碼器的平均計數值*/
uint16_t wheel_get_count()
{return (count_save(4)+count_save(5)+count_save(6)+count_save(7))/4;
}/*** @brief 編碼器計數保存與讀取函數* @param cmd 命令碼，0-3 為計數加 1，4-7 為讀取計數，8 為重置計數* @return 根據命令碼返回相應計數值，無返回值命令返回 0*/
int32_t count_save(uint8_t cmd)
{static int32_t count[4]={0};if(cmd<4){count[cmd]++;}else if(cmd<8){return count[cmd-4];}else if(cmd==8){for(uint8_t i=0;i<4;i++)count[i]=0;}return 0;
}/*** @brief GPIO 外部中斷回調函數* @param GPIO_Pin 觸發中斷的 GPIO 引腳*/
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {switch (GPIO_Pin) {case ENCODER_CHA_1_PIN:count_save(0);break;case ENCODER_CHA_2_PIN:count_save(1);break;case ENCODER_CHA_3_PIN:count_save(2);break;case ENCODER_CHA_4_PIN:count_save(3);break;default:break;}__HAL_GPIO_EXTI_CLEAR_IT(GPIO_Pin);
}/*** @brief 獲取當前角度值* @return 當前角度值，調用 get_xyz_z 函數獲取*/
uint16_t get_angle_value()
{return (uint16_t)get_xyz_z();
}

5.防陀螺儀角度突變函數

我們的目標角度是355度，但是當前是5度，我們應該是往0處突變成359，而不是旋轉一周，所以需要旋轉突變輔助。

/*** @brief 計算角度差值* @param target 目標角度* @param current 當前角度* @return 角度差值*/
int16_t calculate_angle_diff(uint16_t target, uint16_t current) {int16_t diff = (int16_t)(target - current);diff = (diff + 180) % 360;if (diff < 0) {diff += 360;}diff -= 180;
#if GYRO_POLARITY == 0diff = -diff;
#endifreturn diff;
}

三.功能函數

1.基本開環函數

（1）前進

/*** @brief 開環前進* @param speed 目標速度*/
void wheel_go_forward_openloop(uint16_t speed) {tb6612_set_direction(1, 0);tb6612_set_direction(2, 0);tb6612_set_direction(3, 0);tb6612_set_direction(4, 0);set_all_pwm_channels(speed);
}

（2）后退

/*** @brief 開環后退* @param speed 目標速度*/
void wheel_go_back_openloop(uint16_t speed) {tb6612_set_direction(1, 1);tb6612_set_direction(2, 1);tb6612_set_direction(3, 1);tb6612_set_direction(4, 1);set_all_pwm_channels(speed);
}

（3）左移

/*** @brief 開環左移* @param speed 目標速度*/
void wheel_go_left_openloop(uint16_t speed) {tb6612_set_direction(1, 1);tb6612_set_direction(2, 0);tb6612_set_direction(3, 0);tb6612_set_direction(4, 1);set_all_pwm_channels(speed);
}

（4）右移

/*** @brief 開環右移* @param speed 目標速度*/
void wheel_go_right_openloop(uint16_t speed) {tb6612_set_direction(1, 0);tb6612_set_direction(2, 1);tb6612_set_direction(3, 1);tb6612_set_direction(4, 0);set_all_pwm_channels(speed);
}

（5）自旋轉往左

/*** @brief 開環左轉* @param speed 目標速度*/
void wheel_rotate_left_openloop(uint16_t speed) {tb6612_set_direction(1, 1);tb6612_set_direction(2, 1);tb6612_set_direction(3, 0);tb6612_set_direction(4, 0);set_all_pwm_channels(speed);
}

（6）自旋轉向右

/*** @brief 開環右轉* @param speed 目標速度*/
void wheel_rotate_right_openloop(uint16_t speed) {tb6612_set_direction(1, 0);tb6612_set_direction(2, 0);tb6612_set_direction(3, 1);tb6612_set_direction(4, 1);set_all_pwm_channels(speed);
}

2.帶編碼的開環函數

這個直接走指定距離

（1）前進

/*** @brief 帶編碼器開環前進* @param count 目標編碼器計數* @param speed 目標速度*/
void wheel_go_forward_count_open(uint16_t count, uint16_t speed)
{// 重置編碼器計數wheel_begin_go();// 等待直到編碼器計數達到目標值while (wheel_get_count() < count) {wheel_go_forward_openloop(speed);}// 停止電機set_all_pwm_channels(0);
}

（2）后退

/*** @brief 帶編碼器開環后退* @param count 目標編碼器計數* @param speed 目標速度*/
void wheel_go_back_count_open(uint16_t count, uint16_t speed) {wheel_begin_go();while (wheel_get_count() < count) {wheel_go_back_openloop(speed);}set_all_pwm_channels(0);
}

（3）左移

/*** @brief 帶編碼器開環左移* @param count 目標編碼器計數* @param speed 目標速度*/
void wheel_go_left_count_open(uint16_t count, uint16_t speed) {wheel_begin_go();while (wheel_get_count() < count) {wheel_go_left_openloop(speed);}set_all_pwm_channels(0);
}

（4）右移

/*** @brief 帶編碼器開環右移* @param count 目標編碼器計數* @param speed 目標速度*/
void wheel_go_right_count_open(uint16_t count, uint16_t speed) {wheel_begin_go();while (wheel_get_count() < count) {wheel_go_right_openloop(speed);}set_all_pwm_channels(0);
}

3.陀螺儀閉環函數

這種是根據陀螺儀變化的，需要在while()里執行

（1）前進

/*** @brief 直行控制(帶陀螺儀閉環)* @param now_z 當前角度* @param speed 目標速度*/
void wheel_go_forward(uint16_t now_z, uint16_t speed) {PDController pd = {STRAIGHT_KP, STRAIGHT_KD, 0};tb6612_set_direction(1, 0);tb6612_set_direction(2, 0);tb6612_set_direction(3, 0);tb6612_set_direction(4, 0);uint16_t z = get_angle_value();int16_t diff = calculate_angle_diff(now_z, z);float derivative = diff - pd.prev_error;float output = pd.kp * diff + pd.kd * derivative;pd.prev_error = diff;uint16_t adjustment = (uint16_t)fabs(output);adjustment = (adjustment > speed) ? speed : adjustment;if (diff == 0) {set_all_pwm_channels(speed);} else if (diff > 0) {uint16_t left_speed = speed;uint16_t right_speed = speed - adjustment;set_all_pwm_channels_separate(left_speed, left_speed, right_speed, right_speed);} else {uint16_t left_speed = speed - adjustment;uint16_t right_speed = speed;set_all_pwm_channels_separate(left_speed, left_speed, right_speed, right_speed);}
}

（2）后退


/*** @brief 后退控制(帶陀螺儀閉環)* @param now_z 當前角度* @param speed 目標速度*/
void wheel_go_back(uint16_t now_z, uint16_t speed) {PDController pd = {BACK_KP, BACK_KD, 0};tb6612_set_direction(1, 1);tb6612_set_direction(2, 1);tb6612_set_direction(3, 1);tb6612_set_direction(4, 1);uint16_t z = get_angle_value();int16_t diff = calculate_angle_diff(now_z, z);float derivative = diff - pd.prev_error;float output = pd.kp * diff + pd.kd * derivative;pd.prev_error = diff;uint16_t adjustment = (uint16_t)fabs(output);adjustment = (adjustment > speed) ? speed : adjustment;if (diff == 0) {set_all_pwm_channels(speed);} else if (diff > 0) {uint16_t left_speed = speed - adjustment;uint16_t right_speed = speed;set_all_pwm_channels_separate(left_speed, left_speed, right_speed, right_speed);} else {uint16_t left_speed = speed;uint16_t right_speed = speed - adjustment;set_all_pwm_channels_separate(left_speed, left_speed, right_speed, right_speed);}
}

（3）左移


/*** @brief 左移控制(帶陀螺儀閉環)* @param now_z 當前角度* @param speed 目標速度*/
void wheel_go_left(uint16_t now_z, uint16_t speed) {PDController pd = {LEFT_KP, LEFT_KD, 0};tb6612_set_direction(1, 1);tb6612_set_direction(2, 0);tb6612_set_direction(3, 0);tb6612_set_direction(4, 1);uint16_t z = get_angle_value();int16_t diff = calculate_angle_diff(now_z, z);float derivative = diff - pd.prev_error;float output = pd.kp * diff + pd.kd * derivative;pd.prev_error = diff;uint16_t adjustment = (uint16_t)fabs(output);adjustment = (adjustment > speed) ? speed : adjustment;if (diff == 0) {set_all_pwm_channels(speed);} else if (diff > 0) {uint16_t left_speed = speed;uint16_t right_speed = speed - adjustment;set_all_pwm_channels_separate(left_speed, left_speed, right_speed, right_speed);} else {uint16_t left_speed = speed - adjustment;uint16_t right_speed = speed;set_all_pwm_channels_separate(left_speed, left_speed, right_speed, right_speed);}
}

（4）右移

/*** @brief 右移控制(帶陀螺儀閉環)* @param now_z 當前角度* @param speed 目標速度*/
void wheel_go_right(uint16_t now_z, uint16_t speed) {PDController pd = {RIGHT_KP, RIGHT_KD, 0};tb6612_set_direction(1, 0);tb6612_set_direction(2, 1);tb6612_set_direction(3, 1);tb6612_set_direction(4, 0);uint16_t z = get_angle_value();int16_t diff = calculate_angle_diff(now_z, z);float derivative = diff - pd.prev_error;float output = pd.kp * diff + pd.kd * derivative;pd.prev_error = diff;uint16_t adjustment = (uint16_t)fabs(output);adjustment = (adjustment > speed) ? speed : adjustment;if (diff == 0) {set_all_pwm_channels(speed);} else if (diff > 0) {uint16_t left_speed = speed - adjustment;uint16_t right_speed = speed;set_all_pwm_channels_separate(left_speed, left_speed, right_speed, right_speed);} else {uint16_t left_speed = speed;uint16_t right_speed = speed - adjustment;set_all_pwm_channels_separate(left_speed, left_speed, right_speed, right_speed);}
}

（5）自旋轉

其中左轉為正


/*** @brief 旋轉控制(帶陀螺儀閉環)* @param angle 目標旋轉角度(正值為左轉，負值為右轉)*/
void wheel_rotate(int16_t angle) {PDController pd = {ROTATE_KP, ROTATE_KD, 0};uint16_t initial_z = get_angle_value();uint16_t target_z = (uint16_t)((initial_z + angle + 360) % 360);if (angle < 0) {tb6612_set_direction(1, 0);tb6612_set_direction(2, 0);tb6612_set_direction(3, 1);tb6612_set_direction(4, 1);} else {tb6612_set_direction(1, 1);tb6612_set_direction(2, 1);tb6612_set_direction(3, 0);tb6612_set_direction(4, 0);}set_all_pwm_channels(0);while (1) {uint16_t z = get_angle_value();int16_t diff = calculate_angle_diff(target_z, z);if (abs(diff) < 2) {break;}float derivative = diff - pd.prev_error;float output = pd.kp * diff + pd.kd * derivative;pd.prev_error = diff;uint16_t speed = (uint16_t)fabs(output);if (speed < MIN_ROTATION_DUTY_CYCLE) {speed = MIN_ROTATION_DUTY_CYCLE;} else if (speed > ROTATION_SPEED) {speed = ROTATION_SPEED;}set_all_pwm_channels(speed);}set_all_pwm_channels(0);
}

4.帶編碼的陀螺儀閉環函數

這種的話是帶陀螺儀返款的運動，同時根據設定值運動到指定值停止

（1）前進

/*** @brief 帶編碼器閉環直行* @param count 目標編碼器計數* @param speed 目標速度*/
void wheel_go_forward_count_close(uint16_t count, uint16_t speed) {wheel_begin_go();uint16_t z = get_angle_value();while (wheel_get_count() < count) {wheel_go_forward(z, speed);}set_all_pwm_channels(0);
}

（2）后退

/*** @brief 帶編碼器閉環后退* @param count 目標編碼器計數* @param speed 目標速度*/
void wheel_go_back_count_close(uint16_t count, uint16_t speed) {wheel_begin_go();uint16_t z = get_angle_value();while (wheel_get_count() < count) {wheel_go_back(z, speed);}set_all_pwm_channels(0);
}

（3）左移

/*** @brief 帶編碼器閉環左移* @param count 目標編碼器計數* @param speed 目標速度*/
void wheel_go_left_count_close(uint16_t count, uint16_t speed) {wheel_begin_go();uint16_t z = get_angle_value();while (wheel_get_count() < count) {wheel_go_left(z, speed);}set_all_pwm_channels(0);
}

（4）右移

/*** @brief 帶編碼器閉環右移* @param count 目標編碼器計數* @param speed 目標速度*/
void wheel_go_right_count_close(uint16_t count, uint16_t speed) {wheel_begin_go();uint16_t z = get_angle_value();while (wheel_get_count() < count) {wheel_go_right(z, speed);}set_all_pwm_channels(0);
}

四.測試函數

通過調用測試函數，來判斷輪子的方向極性是否正確


/*** @brief 輪子極性測試函數，依次調用開環和閉環控制函數測試輪子運動極性*/
void wheel_polarity_test(void) {const uint16_t distance = 1000;const uint16_t speed = 300;wheel_go_forward_openloop(0);// 開環測試// 1234 編號輪子依次正轉set_all_pwm_channels_separate(300, 0, 0, 0);HAL_Delay(500);set_all_pwm_channels_separate(0, 300, 0, 0);HAL_Delay(500);set_all_pwm_channels_separate(0, 0, 300, 0);HAL_Delay(500);set_all_pwm_channels_separate(0, 0, 0, 300);HAL_Delay(500);wheel_stop();HAL_Delay(500);// 左轉wheel_rotate_left_openloop(speed);HAL_Delay(500);// 右轉wheel_rotate_right_openloop(speed);HAL_Delay(500);// 正左轉wheel_go_left_openloop(speed);HAL_Delay(500);// 正右轉wheel_go_right_openloop(speed);HAL_Delay(500);// 前進wheel_go_forward_openloop(speed);HAL_Delay(500);// 后退wheel_go_back_openloop(speed);HAL_Delay(500);// 閉環距離測試// 前進wheel_go_forward_count_close(distance, speed);HAL_Delay(500);// 后退wheel_go_back_count_close(distance, speed);HAL_Delay(500);// 左移wheel_go_left_count_close(distance, speed);HAL_Delay(500);// 右移wheel_go_right_count_close(distance, speed);HAL_Delay(500);//左轉90度wheel_rotate(90);wheel_stop();HAL_Delay(500);//右轉90度wheel_rotate(-90);wheel_stop();HAL_Delay(500);}

五.使用方法

1.首先確定輪子的極性，把下列引腳對應好


/* 硬件相關宏定義 
1     4
2     3
*/
// TB6612 方向引腳定義
#define MOTOR1_PIN_A_PORT GPIOA
#define MOTOR1_PIN_A_PIN (1 << 8)
#define MOTOR1_PIN_B_PORT GPIOC
#define MOTOR1_PIN_B_PIN (1 << 9)#define MOTOR2_PIN_A_PORT GPIOC
#define MOTOR2_PIN_A_PIN (1 << 8)
#define MOTOR2_PIN_B_PORT GPIOD
#define MOTOR2_PIN_B_PIN (1 << 13)#define MOTOR3_PIN_A_PORT GPIOD
#define MOTOR3_PIN_A_PIN (1 << 9)
#define MOTOR3_PIN_B_PORT GPIOD
#define MOTOR3_PIN_B_PIN (1 << 10)#define MOTOR4_PIN_A_PORT GPIOD
#define MOTOR4_PIN_A_PIN (1 << 11)
#define MOTOR4_PIN_B_PORT GPIOD
#define MOTOR4_PIN_B_PIN (1 << 12)/* 定時器相關宏定義 */
#define PWM_TIMER &htim1
#define PWM_CHANNEL_1 TIM_CHANNEL_1
#define PWM_CHANNEL_2 TIM_CHANNEL_2
#define PWM_CHANNEL_3 TIM_CHANNEL_3
#define PWM_CHANNEL_4 TIM_CHANNEL_4/* 編碼器相關宏定義 */
#define ENCODER_CHA_1_PIN GPIO_PIN_4
#define ENCODER_CHA_1_PORT GPIOA
#define ENCODER_CHA_2_PIN GPIO_PIN_2
#define ENCODER_CHA_2_PORT GPIOB
#define ENCODER_CHA_3_PIN GPIO_PIN_6
#define ENCODER_CHA_3_PORT GPIOE
#define ENCODER_CHA_4_PIN GPIO_PIN_12
#define ENCODER_CHA_4_PORT GPIOE

然后這個函數的返回值修改成自己陀螺儀的返回值函數

/*** @brief 獲取當前角度值* @return 當前角度值，調用 get_xyz_z 函數獲取*/
uint16_t get_angle_value()
{return (uint16_t)get_xyz_z();
}

2.極性正確測試

添加頭文件wheel.h，在主函數添加初始化函數wheel_init();，然后在while（1）中只運行調用測試程序wheel_polarity_test()?，看看輪子是否按著1234編號運動，不是請修改

3.陀螺儀極性

隨后看看左轉和右轉是不是以最近角度旋轉，如果不是那么就是極性反了

基本上沒有問題后就可以正常使用了

六.完整代碼

wheel.c

#include "wheel.h"
#include "gpio.h"
#include "hand.h"
#include "tim.h"
#include "wtkj.h"
#include <stdio.h>
#include <math.h>/* ==================== 輔助函數 ==================== *//*** @brief 確保計算后的速度不小于0* @param speed 當前速度值* @param error 誤差值* @return 調整后的速度值*/
uint16_t clamp_speed(uint16_t speed, uint16_t error) {if (speed > error) {return speed - error;}return 0;
}/*** @brief 限制速度在合理范圍* @param speed 當前速度值* @param max_speed 最大速度限制* @return 調整后的速度值*/
uint16_t limit_speed(uint16_t speed, uint16_t max_speed) {if (speed > max_speed) {return max_speed;}return speed;
}/* ==================== 基礎控制函數 ==================== *//*** @brief 設置四個通道PWM占空比(相同值)* @param speed PWM占空比值*/
void set_all_pwm_channels(uint16_t speed) {__HAL_TIM_SetCompare(PWM_TIMER, PWM_CHANNEL_1, speed);__HAL_TIM_SetCompare(PWM_TIMER, PWM_CHANNEL_2, speed);__HAL_TIM_SetCompare(PWM_TIMER, PWM_CHANNEL_3, speed);__HAL_TIM_SetCompare(PWM_TIMER, PWM_CHANNEL_4, speed);
}/*** @brief 設置四個通道PWM占空比(不同值)* @param speed1 通道1PWM值* @param speed2 通道2PWM值* @param speed3 通道3PWM值* @param speed4 通道4PWM值*/
void set_all_pwm_channels_separate(uint16_t speed1, uint16_t speed2, uint16_t speed3, uint16_t speed4) {__HAL_TIM_SetCompare(PWM_TIMER, PWM_CHANNEL_1, speed1);__HAL_TIM_SetCompare(PWM_TIMER, PWM_CHANNEL_2, speed2);__HAL_TIM_SetCompare(PWM_TIMER, PWM_CHANNEL_3, speed3);__HAL_TIM_SetCompare(PWM_TIMER, PWM_CHANNEL_4, speed4);
}/*** @brief 控制TB6612電機方向* @param motor_num 電機編號(1-4)* @param polarity 方向(0或1)*/
void tb6612_set_direction(uint8_t motor_num, uint8_t polarity) {switch (motor_num) {case 1:if (polarity == 0) {HAL_GPIO_WritePin(MOTOR1_PIN_A_PORT, MOTOR1_PIN_A_PIN, 1);HAL_GPIO_WritePin(MOTOR1_PIN_B_PORT, MOTOR1_PIN_B_PIN, 0);} else {HAL_GPIO_WritePin(MOTOR1_PIN_A_PORT, MOTOR1_PIN_A_PIN, 0);HAL_GPIO_WritePin(MOTOR1_PIN_B_PORT, MOTOR1_PIN_B_PIN, 1);}break;case 2:if (polarity == 0) {HAL_GPIO_WritePin(MOTOR2_PIN_A_PORT, MOTOR2_PIN_A_PIN, 1);HAL_GPIO_WritePin(MOTOR2_PIN_B_PORT, MOTOR2_PIN_B_PIN, 0);} else {HAL_GPIO_WritePin(MOTOR2_PIN_A_PORT, MOTOR2_PIN_A_PIN, 0);HAL_GPIO_WritePin(MOTOR2_PIN_B_PORT, MOTOR2_PIN_B_PIN, 1);}break;case 3:if (polarity == 0) {HAL_GPIO_WritePin(MOTOR3_PIN_A_PORT, MOTOR3_PIN_A_PIN, 1);HAL_GPIO_WritePin(MOTOR3_PIN_B_PORT, MOTOR3_PIN_B_PIN, 0);} else {HAL_GPIO_WritePin(MOTOR3_PIN_A_PORT, MOTOR3_PIN_A_PIN, 0);HAL_GPIO_WritePin(MOTOR3_PIN_B_PORT, MOTOR3_PIN_B_PIN, 1);}break;case 4:if (polarity == 0) {HAL_GPIO_WritePin(MOTOR4_PIN_A_PORT, MOTOR4_PIN_A_PIN, 1);HAL_GPIO_WritePin(MOTOR4_PIN_B_PORT, MOTOR4_PIN_B_PIN, 0);} else {HAL_GPIO_WritePin(MOTOR4_PIN_A_PORT, MOTOR4_PIN_A_PIN, 0);HAL_GPIO_WritePin(MOTOR4_PIN_B_PORT, MOTOR4_PIN_B_PIN, 1);}break;default:break;}
}/*** @brief 初始化輪子控制模塊*/void wheel_init() {HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_2);HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_3);HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_4); set_all_pwm_channels(0);}/*** @brief 停止所有輪子*/void wheel_stop() {set_all_pwm_channels(0);}/*** @brief 開始運動，重置編碼器計數*/
void wheel_begin_go()
{count_save(8);
}/*** @brief 獲取編碼器平均計數* @return 四個編碼器的平均計數值*/
uint16_t     wheel_get_count()
{return (count_save(4)+count_save(5)+count_save(6)+count_save(7))/4;
}/*** @brief 編碼器計數保存與讀取函數* @param cmd 命令碼，0-3 為計數加 1，4-7 為讀取計數，8 為重置計數* @return 根據命令碼返回相應計數值，無返回值命令返回 0*/
int32_t count_save(uint8_t cmd)
{static int32_t count[4]={0};if(cmd<4){count[cmd]++;}else if(cmd<8){return count[cmd-4];}else if(cmd==8){for(uint8_t i=0;i<4;i++)count[i]=0;}return 0;
}/*** @brief GPIO 外部中斷回調函數* @param GPIO_Pin 觸發中斷的 GPIO 引腳*/
void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) {switch (GPIO_Pin) {case ENCODER_CHA_1_PIN:count_save(0);break;case ENCODER_CHA_2_PIN:count_save(1);break;case ENCODER_CHA_3_PIN:count_save(2);break;case ENCODER_CHA_4_PIN:count_save(3);break;default:break;}__HAL_GPIO_EXTI_CLEAR_IT(GPIO_Pin);
}/*** @brief 獲取當前角度值* @return 當前角度值，調用 get_xyz_z 函數獲取*/
uint16_t get_angle_value()
{return (uint16_t)get_xyz_z();
}/* ==================== 閉環控制函數 ==================== *//*** @brief 計算角度差值* @param target 目標角度* @param current 當前角度* @return 角度差值*/
int16_t calculate_angle_diff(uint16_t target, uint16_t current) {int16_t diff = (int16_t)(target - current);diff = (diff + 180) % 360;if (diff < 0) {diff += 360;}diff -= 180;
#if GYRO_POLARITY == 0diff = -diff;
#endifreturn diff;
}/*** @brief 后退控制(帶陀螺儀閉環)* @param now_z 當前角度* @param speed 目標速度*/void wheel_go_back(uint16_t now_z, uint16_t speed) {PDController pd = {BACK_KP, BACK_KD, 0};tb6612_set_direction(1, 1);tb6612_set_direction(2, 1);tb6612_set_direction(3, 1);tb6612_set_direction(4, 1);uint16_t z = get_angle_value();int16_t diff = calculate_angle_diff(now_z, z);float derivative = diff - pd.prev_error;float output = pd.kp * diff + pd.kd * derivative;pd.prev_error = diff;uint16_t adjustment = (uint16_t)fabs(output);adjustment = (adjustment > speed) ? speed : adjustment;if (diff == 0) {set_all_pwm_channels(speed);} else if (diff > 0) {uint16_t left_speed = speed - adjustment;uint16_t right_speed = speed;set_all_pwm_channels_separate(left_speed, left_speed, right_speed, right_speed);} else {uint16_t left_speed = speed;uint16_t right_speed = speed - adjustment;set_all_pwm_channels_separate(left_speed, left_speed, right_speed, right_speed);}}/*** @brief 直行控制(帶陀螺儀閉環)* @param now_z 當前角度* @param speed 目標速度*/
void wheel_go_forward(uint16_t now_z, uint16_t speed) {PDController pd = {STRAIGHT_KP, STRAIGHT_KD, 0};tb6612_set_direction(1, 0);tb6612_set_direction(2, 0);tb6612_set_direction(3, 0);tb6612_set_direction(4, 0);uint16_t z = get_angle_value();int16_t diff = calculate_angle_diff(now_z, z);float derivative = diff - pd.prev_error;float output = pd.kp * diff + pd.kd * derivative;pd.prev_error = diff;uint16_t adjustment = (uint16_t)fabs(output);adjustment = (adjustment > speed) ? speed : adjustment;if (diff == 0) {set_all_pwm_channels(speed);} else if (diff > 0) {uint16_t left_speed = speed;uint16_t right_speed = speed - adjustment;set_all_pwm_channels_separate(left_speed, left_speed, right_speed, right_speed);} else {uint16_t left_speed = speed - adjustment;uint16_t right_speed = speed;set_all_pwm_channels_separate(left_speed, left_speed, right_speed, right_speed);}
}/*** @brief 左移控制(帶陀螺儀閉環)* @param now_z 當前角度* @param speed 目標速度*/
void wheel_go_left(uint16_t now_z, uint16_t speed) {PDController pd = {LEFT_KP, LEFT_KD, 0};tb6612_set_direction(1, 1);tb6612_set_direction(2, 0);tb6612_set_direction(3, 0);tb6612_set_direction(4, 1);uint16_t z = get_angle_value();int16_t diff = calculate_angle_diff(now_z, z);float derivative = diff - pd.prev_error;float output = pd.kp * diff + pd.kd * derivative;pd.prev_error = diff;uint16_t adjustment = (uint16_t)fabs(output);adjustment = (adjustment > speed) ? speed : adjustment;if (diff == 0) {set_all_pwm_channels(speed);} else if (diff > 0) {uint16_t left_speed = speed;uint16_t right_speed = speed - adjustment;set_all_pwm_channels_separate(left_speed, left_speed, right_speed, right_speed);} else {uint16_t left_speed = speed - adjustment;uint16_t right_speed = speed;set_all_pwm_channels_separate(left_speed, left_speed, right_speed, right_speed);}
}/*** @brief 右移控制(帶陀螺儀閉環)* @param now_z 當前角度* @param speed 目標速度*/
void wheel_go_right(uint16_t now_z, uint16_t speed) {PDController pd = {RIGHT_KP, RIGHT_KD, 0};tb6612_set_direction(1, 0);tb6612_set_direction(2, 1);tb6612_set_direction(3, 1);tb6612_set_direction(4, 0);uint16_t z = get_angle_value();int16_t diff = calculate_angle_diff(now_z, z);float derivative = diff - pd.prev_error;float output = pd.kp * diff + pd.kd * derivative;pd.prev_error = diff;uint16_t adjustment = (uint16_t)fabs(output);adjustment = (adjustment > speed) ? speed : adjustment;if (diff == 0) {set_all_pwm_channels(speed);} else if (diff > 0) {uint16_t left_speed = speed - adjustment;uint16_t right_speed = speed;set_all_pwm_channels_separate(left_speed, left_speed, right_speed, right_speed);} else {uint16_t left_speed = speed;uint16_t right_speed = speed - adjustment;set_all_pwm_channels_separate(left_speed, left_speed, right_speed, right_speed);}
}/*** @brief 旋轉控制(帶陀螺儀閉環)* @param angle 目標旋轉角度(正值為左轉，負值為右轉)*/
void wheel_rotate(int16_t angle) {PDController pd = {ROTATE_KP, ROTATE_KD, 0};uint16_t initial_z = get_angle_value();uint16_t target_z = (uint16_t)((initial_z + angle + 360) % 360);if (angle < 0) {tb6612_set_direction(1, 0);tb6612_set_direction(2, 0);tb6612_set_direction(3, 1);tb6612_set_direction(4, 1);} else {tb6612_set_direction(1, 1);tb6612_set_direction(2, 1);tb6612_set_direction(3, 0);tb6612_set_direction(4, 0);}set_all_pwm_channels(0);while (1) {uint16_t z = get_angle_value();int16_t diff = calculate_angle_diff(target_z, z);if (abs(diff) < 2) {break;}float derivative = diff - pd.prev_error;float output = pd.kp * diff + pd.kd * derivative;pd.prev_error = diff;uint16_t speed = (uint16_t)fabs(output);if (speed < MIN_ROTATION_DUTY_CYCLE) {speed = MIN_ROTATION_DUTY_CYCLE;} else if (speed > ROTATION_SPEED) {speed = ROTATION_SPEED;}set_all_pwm_channels(speed);}set_all_pwm_channels(0);
}/* ==================== 開環控制函數 ==================== *//*** @brief 開環前進* @param speed 目標速度*/
void wheel_go_forward_openloop(uint16_t speed) {tb6612_set_direction(1, 0);tb6612_set_direction(2, 0);tb6612_set_direction(3, 0);tb6612_set_direction(4, 0);set_all_pwm_channels(speed);
}/*** @brief 開環后退* @param speed 目標速度*/
void wheel_go_back_openloop(uint16_t speed) {tb6612_set_direction(1, 1);tb6612_set_direction(2, 1);tb6612_set_direction(3, 1);tb6612_set_direction(4, 1);set_all_pwm_channels(speed);
}/*** @brief 開環左移* @param speed 目標速度*/
void wheel_go_left_openloop(uint16_t speed) {tb6612_set_direction(1, 1);tb6612_set_direction(2, 0);tb6612_set_direction(3, 0);tb6612_set_direction(4, 1);set_all_pwm_channels(speed);
}/*** @brief 開環右移* @param speed 目標速度*/
void wheel_go_right_openloop(uint16_t speed) {tb6612_set_direction(1, 0);tb6612_set_direction(2, 1);tb6612_set_direction(3, 1);tb6612_set_direction(4, 0);set_all_pwm_channels(speed);
}/*** @brief 開環左轉* @param speed 目標速度*/
void wheel_rotate_left_openloop(uint16_t speed) {tb6612_set_direction(1, 1);tb6612_set_direction(2, 1);tb6612_set_direction(3, 0);tb6612_set_direction(4, 0);set_all_pwm_channels(speed);
}/*** @brief 開環右轉* @param speed 目標速度*/
void wheel_rotate_right_openloop(uint16_t speed) {tb6612_set_direction(1, 0);tb6612_set_direction(2, 0);tb6612_set_direction(3, 1);tb6612_set_direction(4, 1);set_all_pwm_channels(speed);
}/* ==================== 帶編碼器的閉環控制函數 ==================== *//*** @brief 帶編碼器閉環直行* @param count 目標編碼器計數* @param speed 目標速度*/
void wheel_go_forward_count_close(uint16_t count, uint16_t speed) {wheel_begin_go();uint16_t z = get_angle_value();while (wheel_get_count() < count) {wheel_go_forward(z, speed);}set_all_pwm_channels(0);
}/*** @brief 帶編碼器閉環后退* @param count 目標編碼器計數* @param speed 目標速度*/
void wheel_go_back_count_close(uint16_t count, uint16_t speed) {wheel_begin_go();uint16_t z = get_angle_value();while (wheel_get_count() < count) {wheel_go_back(z, speed);}set_all_pwm_channels(0);
}/*** @brief 帶編碼器閉環左移* @param count 目標編碼器計數* @param speed 目標速度*/
void wheel_go_left_count_close(uint16_t count, uint16_t speed) {wheel_begin_go();uint16_t z = get_angle_value();while (wheel_get_count() < count) {wheel_go_left(z, speed);}set_all_pwm_channels(0);
}/*** @brief 帶編碼器閉環右移* @param count 目標編碼器計數* @param speed 目標速度*/void wheel_go_right_count_close(uint16_t count, uint16_t speed) {wheel_begin_go();uint16_t z = get_angle_value();while (wheel_get_count() < count) {wheel_go_right(z, speed);}set_all_pwm_channels(0);}/*** @brief 帶編碼器開環前進* @param count 目標編碼器計數* @param speed 目標速度*/
void wheel_go_forward_count_open(uint16_t count, uint16_t speed)
{// 重置編碼器計數wheel_begin_go();// 等待直到編碼器計數達到目標值while (wheel_get_count() < count) {wheel_go_forward_openloop(speed);}// 停止電機set_all_pwm_channels(0);
}/*** @brief 帶編碼器開環后退* @param count 目標編碼器計數* @param speed 目標速度*/
void wheel_go_back_count_open(uint16_t count, uint16_t speed) {wheel_begin_go();while (wheel_get_count() < count) {wheel_go_back_openloop(speed);}set_all_pwm_channels(0);
}/*** @brief 帶編碼器開環左移* @param count 目標編碼器計數* @param speed 目標速度*/
void wheel_go_left_count_open(uint16_t count, uint16_t speed) {wheel_begin_go();while (wheel_get_count() < count) {wheel_go_left_openloop(speed);}set_all_pwm_channels(0);
}/*** @brief 帶編碼器開環右移* @param count 目標編碼器計數* @param speed 目標速度*/void wheel_go_right_count_open(uint16_t count, uint16_t speed) {wheel_begin_go();while (wheel_get_count() < count) {wheel_go_right_openloop(speed);}set_all_pwm_channels(0);}/* ==================== 輪子極性測試函數 ==================== *//*** @brief 輪子極性測試函數，依次調用開環和閉環控制函數測試輪子運動極性*/
void wheel_polarity_test(void) {const uint16_t distance = 1000;const uint16_t speed = 300;wheel_go_forward_openloop(0);// 開環測試// 1234 編號輪子依次正轉set_all_pwm_channels_separate(300, 0, 0, 0);HAL_Delay(500);set_all_pwm_channels_separate(0, 300, 0, 0);HAL_Delay(500);set_all_pwm_channels_separate(0, 0, 300, 0);HAL_Delay(500);set_all_pwm_channels_separate(0, 0, 0, 300);HAL_Delay(500);wheel_stop();HAL_Delay(500);// 左轉wheel_rotate_left_openloop(speed);HAL_Delay(500);// 右轉wheel_rotate_right_openloop(speed);HAL_Delay(500);// 正左轉wheel_go_left_openloop(speed);HAL_Delay(500);// 正右轉wheel_go_right_openloop(speed);HAL_Delay(500);// 前進wheel_go_forward_openloop(speed);HAL_Delay(500);// 后退wheel_go_back_openloop(speed);HAL_Delay(500);// 閉環距離測試// 前進wheel_go_forward_count_close(distance, speed);HAL_Delay(500);// 后退wheel_go_back_count_close(distance, speed);HAL_Delay(500);// 左移wheel_go_left_count_close(distance, speed);HAL_Delay(500);// 右移wheel_go_right_count_close(distance, speed);HAL_Delay(500);wheel_rotate(90);wheel_stop();HAL_Delay(500);wheel_rotate(-90);wheel_stop();HAL_Delay(500);}

wheel.h

#ifndef WHEEL_H
#define WHEEL_H/*** @file wheel.h* @brief 輪子控制模塊頭文件*//* 系統頭文件包含 */
#include "main.h"/* 硬件相關宏定義 */
// TB6612 方向引腳定義
#define MOTOR1_PIN_A_PORT GPIOA
#define MOTOR1_PIN_A_PIN (1 << 8)
#define MOTOR1_PIN_B_PORT GPIOC
#define MOTOR1_PIN_B_PIN (1 << 9)#define MOTOR2_PIN_A_PORT GPIOC
#define MOTOR2_PIN_A_PIN (1 << 8)
#define MOTOR2_PIN_B_PORT GPIOD
#define MOTOR2_PIN_B_PIN (1 << 13)#define MOTOR3_PIN_A_PORT GPIOD
#define MOTOR3_PIN_A_PIN (1 << 9)
#define MOTOR3_PIN_B_PORT GPIOD
#define MOTOR3_PIN_B_PIN (1 << 10)#define MOTOR4_PIN_A_PORT GPIOD
#define MOTOR4_PIN_A_PIN (1 << 11)
#define MOTOR4_PIN_B_PORT GPIOD
#define MOTOR4_PIN_B_PIN (1 << 12)/* 定時器相關宏定義 */
#define PWM_TIMER &htim1
#define PWM_CHANNEL_1 TIM_CHANNEL_1
#define PWM_CHANNEL_2 TIM_CHANNEL_2
#define PWM_CHANNEL_3 TIM_CHANNEL_3
#define PWM_CHANNEL_4 TIM_CHANNEL_4/* 編碼器相關宏定義 */
#define ENCODER_CHA_1_PIN GPIO_PIN_4
#define ENCODER_CHA_1_PORT GPIOA
#define ENCODER_CHA_2_PIN GPIO_PIN_2
#define ENCODER_CHA_2_PORT GPIOB
#define ENCODER_CHA_3_PIN GPIO_PIN_6
#define ENCODER_CHA_3_PORT GPIOE
#define ENCODER_CHA_4_PIN GPIO_PIN_12
#define ENCODER_CHA_4_PORT GPIOE/* 控制參數相關宏定義 */
// 速度控制參數
#define MIN_ROTATION_DUTY_CYCLE 100
#define ROTATION_SPEED 300// PD控制器參數
#define BACK_KP 10        // 后退PD參數
#define BACK_KD 0
#define STRAIGHT_KP 10    // 直行PD參數
#define STRAIGHT_KD 0
#define LEFT_KP 30        // 橫向左轉PD參數
#define LEFT_KD 0
#define RIGHT_KP 30       // 橫向右轉PD參數
#define RIGHT_KD 0
#define ROTATE_KP 5       // 旋轉PD參數
#define ROTATE_KD 0.2// 陀螺儀配置
#define GYRO_POLARITY 0   // 陀螺儀角度極性，0表示角度減，1表示角度加/* 類型定義 */
typedef struct {float kp;            // 比例系數float kd;            // 微分系數float prev_error;    // 上一次誤差
} PDController;/* 函數聲明 */
// 初始化函數
void wheel_init(void);
void wheel_begin_go(void);// 基礎控制函數
void wheel_stop(void);
void set_all_pwm_channels(uint16_t speed);
void set_all_pwm_channels_separate(uint16_t speed1, uint16_t speed2, uint16_t speed3, uint16_t speed4);
void tb6612_set_direction(uint8_t motor_num, uint8_t polarity);// 閉環控制函數
void wheel_go_back(uint16_t now_z, uint16_t speed);
// 修改函數名
void wheel_go_forward(uint16_t now_z, uint16_t speed);
void wheel_go_right(uint16_t now_z, uint16_t speed);
void wheel_go_left(uint16_t now_z, uint16_t speed);
void wheel_rotate(int16_t angle);// 開環控制函數
void wheel_go_forward_openloop(uint16_t speed);
void wheel_go_back_openloop(uint16_t speed);
void wheel_go_left_openloop(uint16_t speed);
void wheel_go_right_openloop(uint16_t speed);
void wheel_rotate_left_openloop(uint16_t speed);
void wheel_rotate_right_openloop(uint16_t speed);// 帶編碼器的閉環控制函數
// 修改函數名
void wheel_go_forward_count_close(uint16_t count, uint16_t speed);
void wheel_go_back_count_close(uint16_t count, uint16_t speed);
void wheel_go_left_count_close(uint16_t count, uint16_t speed);
void wheel_go_right_count_close(uint16_t count, uint16_t speed);// 帶編碼器的開環控制函數
void wheel_go_forward_count_open(uint16_t count, uint16_t speed);
void wheel_go_back_count_open(uint16_t count, uint16_t speed);
void wheel_go_left_count_open(uint16_t count, uint16_t speed);
void wheel_go_right_count_open(uint16_t count, uint16_t speed);// 輔助函數
uint16_t clamp_speed(uint16_t speed, uint16_t error);
uint16_t get_angle_value(void);
uint16_t wheel_get_count(void);
int32_t count_save(uint8_t cmd);
void wheel_polarity_test(void);#endif // WHEEL_H/*
1                42                3設置好對應編號后使用
wheel_polarity_test(void);
這個函數確定極性是否正確，是否按照
1234編號輪子依次正轉
然后
左轉
右轉
正右轉
正左轉
后退
前進
的方式運動，如果錯誤請重新矯正
*/

本文來自互聯網用戶投稿，該文觀點僅代表作者本人，不代表本站立場。本站僅提供信息存儲空間服務，不擁有所有權，不承擔相關法律責任。
如若轉載，請注明出處：http://www.pswp.cn/diannao/85324.shtml
繁體地址，請注明出處：http://hk.pswp.cn/diannao/85324.shtml
英文地址，請注明出處：http://en.pswp.cn/diannao/85324.shtml

如若內容造成侵權/違法違規/事實不符，請聯系多彩編程網進行投訴反饋email:809451989@qq.com，一經查實，立即刪除！

相關文章

文檔核心結構優化（程序C++...）

文檔核心結構優化（程序C++...）

文檔核心結構優化 一、文檔核心結構優化二、C關鍵特性詳解框架2.1 從C到C的范式遷移 三、深度代碼解析模板3.1 現代C特性分層解析 四、C vs C 關鍵差異矩陣五、交互式文檔設計策略5.1 三維學習路徑5.2 代碼缺陷互動區 六、現代C特性演進圖七、性能優化可視化呈現&#xff08;深…
閱讀更多...
PyTorch ——torchvision數據集使用

PyTorch ——torchvision數據集使用

如果下載的很慢&#xff0c;可以試試下面這個
閱讀更多...
純前端實現圖片偽3D視差效果

純前端實現圖片偽3D視差效果

作者&#xff1a;vivo 互聯網前端團隊- Su Ning 本文通過depth-anything獲取圖片的深度圖&#xff0c;同時基于pixi.js&#xff0c;通過著色器編程&#xff0c;實現了通過深度圖驅動的偽3D效果。該方案支持鼠標/手勢與手機陀螺儀雙模式交互&#xff0c;在保證性能的同時&#x…
閱讀更多...
英語寫作中“專注于”focus on、concentrate的用法

英語寫作中“專注于”focus on、concentrate的用法

Focus on在論文寫作中常用&#xff0c;指出研究點&#xff0c;例如&#xff1a; There are three approaches to achieving ID authentication. Our study will focus on ……&#xff08;有三種途徑實現身份認證&#xff0c;我們的研究專注于……&#xff09; concentrate &…
閱讀更多...
go環境配置

go環境配置

下載對應版本的 go 版本 https://go.dev/dl/ 配置 vim ~/.zshrc export GOROOT/usr/local/go export PATH$PATH:$GOROOT/binsource ~/.zshrc >>>>>> go versiongoland 配置&#xff1a; &#x1f50d; 一、什么是GOPATH&#xff1f; GOPATH 是舊的項目結…
閱讀更多...
AI Agent智能體：底層邏輯、原理與大模型關系深度解析·優雅草卓伊凡

AI Agent智能體：底層邏輯、原理與大模型關系深度解析·優雅草卓伊凡

AI Agent智能體&#xff1a;底層邏輯、原理與大模型關系深度解析優雅草卓伊凡 一、AI Agent的底層架構與核心原理 1.1 AI Agent的基本構成要素 AI Agent&#xff08;人工智能代理&#xff09;是一種能夠感知環境、自主決策并執行行動的智能系統。其核心架構包含以下關鍵組件…
閱讀更多...
【手搓一個原生全局loading組件解決頁面閃爍問題】

【手搓一個原生全局loading組件解決頁面閃爍問題】

頁面閃爍效果1 頁面閃爍效果2 封裝一個全局loading組件 class GlobalLoading extends HTMLElement {constructor() {super();this.attachShadow({ mode: open });}connectedCallback() {this.render();this.init();}render() {this.shadowRoot.innerHTML <style>.load…
閱讀更多...
unix/linux source 命令，其高級使用

unix/linux source 命令，其高級使用

就像在物理學中，掌握了基本定律后，我們可以開始研究更復雜的系統和現象，source 的高級用法也是建立在對其基本行為深刻理解之上的。 讓我們一起探索 source 的高級應用領域： 1. 條件化加載 (Conditional Sourcing) 根據某些條件來決定是否 source 一個文件，或者 source…
閱讀更多...
DexGarmentLab 論文翻譯

DexGarmentLab 論文翻譯

單個 專家 演示 裝扮 15 任務 場景 2500+ 服裝 手套 棒球帽 褲子 圍巾 碗 帽子 上衣 外套 服裝-手部交互 捕捉 搖籃 夾緊 平滑 任務 ...... 投擲 懸掛 折疊 ... 多樣化位置 ... 多樣化 變形 ... 多樣化服裝形狀 類別級 一般化 類別級（有或沒有變形） 服裝具有相同結構 變形 生…
閱讀更多...
WPF-Prism學習筆記之 “導航功能和依賴注入“

WPF-Prism學習筆記之 “導航功能和依賴注入“

新建空白模板(Prism) 新建好后會有自動創建ViewModels和Views 在"MainWindow.xaml"文件里面標題去綁定了一個屬性"Title"&#xff0c;而"MainWindowViewModel.cs"里面繼承一個非常重要的"BindbleBase"(prism框架里面非常重要的)。所以…
閱讀更多...
《C++初階之入門基礎》【C++的前世今生】

《C++初階之入門基礎》【C++的前世今生】

【C的前世今生】目錄 前言&#xff1a;---------------起源---------------一、歷史背景二、橫空出世---------------發展---------------三、標準立世C98&#xff1a;首個國際標準版本C03&#xff1a;小修訂版本 四、現代進化C11&#xff1a;現代C的開端C14&#xff1a;對C11的…
閱讀更多...
YOLOv5-入門篇筆記

YOLOv5-入門篇筆記

1.創建環境 conda create -n yolvo5 python3.8 去pytorch.org下載1.8.2的版本。 pip --default-timeout1688 install torch1.8.2 torchvision0.9.2 torchaudio0.8.2 --extra-index-url https://download.pytorch.org/whl/lts/1.8/cu111 github上下載yolov5的zip pip --def…
閱讀更多...
【PostgreSQL 03】PostGIS空間數據深度實戰：從地圖服務到智慧城市

【PostgreSQL 03】PostGIS空間數據深度實戰：從地圖服務到智慧城市

PostGIS空間數據深度實戰&#xff1a;從地圖服務到智慧城市 關鍵詞 PostGIS, 空間數據庫, 地理信息系統, GIS, 空間查詢, 地理分析, 位置服務, 智慧城市, 空間索引, 坐標系統 摘要 PostGIS是PostgreSQL的空間數據擴展&#xff0c;它將普通的關系數據庫轉變為強大的地理信息系統…
閱讀更多...
科技修真的解決方案

科技修真的解決方案

“科技修真”是一個結合現代科技與修真&#xff08;玄幻&#xff09;元素的創新概念&#xff0c;通常出現在科幻或玄幻文學作品中&#xff0c;但也可能指代現實中的科技與傳統文化、超自然理念的融合探索。以下是幾種可能的“科技修真”方案&#xff0c;涵蓋技術實現、文化融合…
閱讀更多...
STM32的HAL編碼流程總結（上部）

STM32的HAL編碼流程總結（上部）

目錄 一、GPIO二、中斷系統三、USART串口通信四、I2C通信五、定時器 一、GPIO 1.選擇調試類型 在SYS中Debug選擇Serial Wire模式 2.選擇時鐘源 在RCC中將HSE和LSH都選擇為內部晶振 3.時鐘樹配置 4.GPIO配置 在芯片圖上選擇開啟的引腳和其功能 配置引腳的各自屬性 5.工…
閱讀更多...
java直接獲取MyBatis將要執行的動態sql命令（不是攔截器方式）

java直接獲取MyBatis將要執行的動態sql命令（不是攔截器方式）

目錄 前言 一. 準備數據 1. 傳輸過來的json條件數據 2. mybatis 配置的動態sql 3. 想要的最終會執行的sql并返回給頁面展示 二. 實現方式 三. 最終代碼 前言 1.在平常開發過程中，MyBatis使用時非常多的，一般情況下我們只需要在控制臺看看MyBatis輸出的日志，要不就是實…
閱讀更多...
機器學習算法-決策樹

機器學習算法-決策樹

今天我們用一個 「相親決策」 的例子來講解決策樹算法&#xff0c;保證你輕松理解原理和實現&#xff01; &#x1f333; 決策樹是什么&#xff1f; 決策樹就像玩 「20個問題」猜謎游戲&#xff1a; 你心里想一個東西&#xff08;比如「蘋果」&#xff09; 朋友通過一系列問題…
閱讀更多...
2025——》VSCode Windows 最新安裝指南/VSCode安裝完成后如何驗證是否成功？2025最新VSCode安裝配置全攻略

2025——》VSCode Windows 最新安裝指南/VSCode安裝完成后如何驗證是否成功？2025最新VSCode安裝配置全攻略

1.VSCode Windows 最新安裝指南： 以下是 2025 年 Windows 系統下安裝 Visual Studio Code（VSCode）的最新指南，結合官方文檔與實際操作經驗整理而成： 一、下載官方安裝包： 1.訪問官網： 打開瀏覽器，進入 VSCode 官方下載頁面https://code.visualstudio.com/Download 2…
閱讀更多...
【Elasticsearch】suggest

【Elasticsearch】suggest

在Elasticsearch中&#xff0c;suggest 是一個非常強大的功能&#xff0c;用于實現自動補全、拼寫糾錯和模糊搜索等功能。它可以幫助用戶更快地找到他們想要的內容&#xff0c;同時提升搜索體驗。以下是關于 suggest 的詳細使用方法和常見場景。 1\. Suggest 的基本概念 sugges…
閱讀更多...
[SAP] 如何查詢當前屏幕的Tcode?

[SAP] 如何查詢當前屏幕的Tcode?

事務代碼Tcode是SAP中到達特定屏幕的快捷路徑 如何查詢以下屏幕的事務碼Tcode&#xff1f; 要瀏覽當前所使用的屏幕的事務碼&#xff0c;可以選擇System | Status 這里的事務代碼是[VA22]&#xff0c;它是Change Quotation的事務代碼
閱讀更多...
最新文章

Copyright @ 2022~2023