智能割草機軟體方案介紹：
功能需求如下：
接下來逐步和大家介紹以上功能的實作：
一、產品創建
- 首先登錄涂鴉智能IoT平臺創建產品，點擊創建產品，在標準類目欄的最下方找到“找不到品類”，點擊進入自定義產品創建頁面，
- 輸入產品名稱和描述，通訊協議選擇WIFI-藍牙，點擊創建產品，
-
在功能定義一欄添加DP點，如下圖所示，本demo添加了標準功能：“方向控制”、以及自定義功能“刀片位置”、“刀片轉速”、“范圍長度設定”、“范圍寬度設定”、“弓字除草”等；功能點可以根據需求自行增減，功能點名稱以及屬性也可根據需求自行修改，
-
進入設備面板，可以選擇自己喜歡的模板或者自己自定義面板，除錯階段推薦選擇開發除錯面板，便于測驗，
-
點開硬體開發一欄，對接方式選擇“涂鴉標準模組MCU SDK開發”，模組選擇CBU Wi-Fi&Bluetooth模組（在實際開發程序中可選擇手上已有的涂鴉模組即可）
-
在下方開發資料處點擊下載全部，其中就包含了涂鴉 MCU-SDK 檔案，
-
至此，產品創建階段已經基本完成，此時可以通過“涂鴉智能”app和虛擬設備體驗dp資料的接收和發送，
二、MCU韌體開發
在進行我們的割草機應用開發之前，我們需要在我們的MCU工程中移植涂鴉 MCU-SDK，移植程序可參考檔案：GD32F4系列單片機移植涂鴉MCU-SDK
移植了MCU-SDK后，再搭配一塊燒錄并授權了通用對接韌體的CBU模組，我們的MCU就具備了連接涂鴉云和上報下發dp點的功能，
同時，本demo工程代碼還移植了FreeRTOS系統以便于開發，
完成準備作業后，正式開始割草機的應用功能開發，
本demo例程地址：github
1.電機、舵機、電調驅動
本demo最主要的器件就是電機舵機和電調了，4個電機各控制了一個小輪，負責實作小車的運動；兩個舵機組合在一起用來控制電調和電調上面的刀片的高低位置；電調則是控制刀片旋轉，同時使轉速可調控，
首先為上述器件撰寫初始化和設定介面，方便后續呼叫，相關的介面都實作在 servo_motor.c檔案中，
- 輸出IO口宏定義，方便后續修改：
#define MOTOR_PORT_1 GPIOA
#define MOTOR_PIN_1 GPIO_PIN_15
#define MOTOR_PORT_1_P GPIOD
#define MOTOR_PIN_1_P GPIO_PIN_0
#define MOTOR_PORT_1_N GPIOD
#define MOTOR_PIN_1_N GPIO_PIN_7
#define MOTOR_PORT_2 GPIOB
#define MOTOR_PIN_2 GPIO_PIN_9
#define MOTOR_PORT_2_P GPIOD
#define MOTOR_PIN_2_P GPIO_PIN_1
#define MOTOR_PORT_2_N GPIOD
#define MOTOR_PIN_2_N GPIO_PIN_2
#define MOTOR_PORT_3 GPIOB
#define MOTOR_PIN_3 GPIO_PIN_10
#define MOTOR_PORT_3_P GPIOD
#define MOTOR_PIN_3_P GPIO_PIN_3
#define MOTOR_PORT_3_N GPIOD
#define MOTOR_PIN_3_N GPIO_PIN_4
#define MOTOR_PORT_4 GPIOB
#define MOTOR_PIN_4 GPIO_PIN_11
#define MOTOR_PORT_4_P GPIOD
#define MOTOR_PIN_4_P GPIO_PIN_8
#define MOTOR_PORT_4_N GPIOD
#define MOTOR_PIN_4_N GPIO_PIN_9
#define MOTOR_CHANNEL_1 TIMER_CH_0
#define MOTOR_CHANNEL_2 TIMER_CH_1
#define MOTOR_CHANNEL_3 TIMER_CH_2
#define MOTOR_CHANNEL_4 TIMER_CH_3
#define SERVO_PORT_1 GPIOC
#define SERVO_PIN_1 GPIO_PIN_6
#define SERVO_PORT_2 GPIOC
#define SERVO_PIN_2 GPIO_PIN_7
#define BLADE_MOTOR_PORT GPIOC
#define BLADE_MOTOR_PIN GPIO_PIN_8
- 初始化介面，呼叫后即打開GPIO時鐘，同時設定輸出IO的普通輸出模式和PWM模式，然后呼叫timer_config去設定PWM的具體引數：
void servo_motor_init(void)
{
rcu_periph_clock_enable(RCU_GPIOA);
rcu_periph_clock_enable(RCU_GPIOB);
rcu_periph_clock_enable(RCU_GPIOC);
rcu_periph_clock_enable(RCU_GPIOD);
/*Configure PD1~8 Output motor Positive and Negative pin to drive wheels_1~4*/
gpio_mode_set(GPIOD, GPIO_MODE_OUTPUT, GPIO_PUPD_NONE, MOTOR_PIN_1_P | MOTOR_PIN_1_N | MOTOR_PIN_2_P | MOTOR_PIN_2_N | MOTOR_PIN_3_P | MOTOR_PIN_3_N | MOTOR_PIN_4_P | MOTOR_PIN_4_N);
gpio_output_options_set(GPIOD, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ,MOTOR_PIN_1_P | MOTOR_PIN_1_N | MOTOR_PIN_2_P | MOTOR_PIN_2_N | MOTOR_PIN_3_P | MOTOR_PIN_3_N | MOTOR_PIN_4_P | MOTOR_PIN_4_N);
gpio_bit_reset(GPIOD, MOTOR_PIN_1_P | MOTOR_PIN_1_N | MOTOR_PIN_2_P | MOTOR_PIN_2_N | MOTOR_PIN_3_P | MOTOR_PIN_3_N | MOTOR_PIN_4_P | MOTOR_PIN_4_N);
/*Configure PA15(TIMER1_CH0) Output PWM pulse to drive wheels_1*/
gpio_mode_set(MOTOR_PORT_1, GPIO_MODE_AF, GPIO_PUPD_NONE, MOTOR_PIN_1);
gpio_output_options_set(MOTOR_PORT_1, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ,MOTOR_PIN_1);
gpio_af_set(MOTOR_PORT_1, GPIO_AF_1, MOTOR_PIN_1);
/*Configure PB9(TIMER1_CH1) Output PWM pulse to drive wheels_2*/
gpio_mode_set(MOTOR_PORT_2, GPIO_MODE_AF, GPIO_PUPD_NONE, MOTOR_PIN_2);
gpio_output_options_set(MOTOR_PORT_2, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ,MOTOR_PIN_2);
gpio_af_set(MOTOR_PORT_2, GPIO_AF_1, MOTOR_PIN_2);
/*Configure PB10(TIMER1_CH2) Output PWM pulse to drive wheels_3*/
gpio_mode_set(MOTOR_PORT_3, GPIO_MODE_AF, GPIO_PUPD_NONE, MOTOR_PIN_3);
gpio_output_options_set(MOTOR_PORT_3, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ,MOTOR_PIN_3);
gpio_af_set(MOTOR_PORT_3, GPIO_AF_1, MOTOR_PIN_3);
/*Configure PB11(TIMER1_CH3) Output PWM pulse to drive wheels_4*/
gpio_mode_set(MOTOR_PORT_4, GPIO_MODE_AF, GPIO_PUPD_NONE, MOTOR_PIN_4);
gpio_output_options_set(MOTOR_PORT_4, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ,MOTOR_PIN_4);
gpio_af_set(MOTOR_PORT_4, GPIO_AF_1, MOTOR_PIN_4);
/*Configure PC6(TIMER2_CH0) Output PWM pulse to drive servo no.1*/
gpio_mode_set(SERVO_PORT_1, GPIO_MODE_AF, GPIO_PUPD_NONE, SERVO_PIN_1);
gpio_output_options_set(SERVO_PORT_1, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ,SERVO_PIN_1);
gpio_af_set(SERVO_PORT_1, GPIO_AF_2, SERVO_PIN_1);
/*Configure PC7(TIMER2_CH1) Output PWM pulse to drive servo no.2*/
gpio_mode_set(SERVO_PORT_2, GPIO_MODE_AF, GPIO_PUPD_NONE, SERVO_PIN_2);
gpio_output_options_set(SERVO_PORT_2, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ,SERVO_PIN_2);
gpio_af_set(SERVO_PORT_2, GPIO_AF_2, SERVO_PIN_2);
/*Configure PC8(TIMER2_CH2) Output PWM pulse to drive blade motor*/
gpio_mode_set(BLADE_MOTOR_PORT, GPIO_MODE_AF, GPIO_PUPD_NONE, BLADE_MOTOR_PIN);
gpio_output_options_set(BLADE_MOTOR_PORT, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ,BLADE_MOTOR_PIN);
gpio_af_set(BLADE_MOTOR_PORT, GPIO_AF_2, BLADE_MOTOR_PIN);
timer_config();
}
void timer_config(void)
{
uint16_t i = 0;
/* TIMER1 configuration: generate PWM signals with different duty cycles:
TIMER1CLK = SystemCoreClock / 120 = 1MHz */
timer_oc_parameter_struct timer_ocintpara;
timer_parameter_struct timer_initpara;
rcu_periph_clock_enable(RCU_TIMER1);
rcu_periph_clock_enable(RCU_TIMER2);
rcu_timer_clock_prescaler_config(RCU_TIMER_PSC_MUL4);
timer_struct_para_init(&timer_initpara);
timer_deinit(TIMER1);
timer_deinit(TIMER2);
/* TIMER1 configuration */
timer_initpara.prescaler = 119;
timer_initpara.alignedmode = TIMER_COUNTER_EDGE;
timer_initpara.counterdirection = TIMER_COUNTER_UP;
timer_initpara.period = 500; /* 500*(1/1MHZ) = 500us */
timer_initpara.clockdivision = TIMER_CKDIV_DIV1;
timer_initpara.repetitioncounter = 0;
timer_init(TIMER1,&timer_initpara);
/* TIMER2 configuration */
timer_initpara.prescaler = 119;
timer_initpara.alignedmode = TIMER_COUNTER_EDGE;
timer_initpara.counterdirection = TIMER_COUNTER_UP;
timer_initpara.period = 20000; /* 20000*(1/1MHZ) = 20ms */
timer_initpara.clockdivision = TIMER_CKDIV_DIV1;
timer_initpara.repetitioncounter = 0;
timer_init(TIMER2,&timer_initpara);
timer_channel_output_struct_para_init(&timer_ocintpara);
timer_ocintpara.ocpolarity = TIMER_OC_POLARITY_HIGH;
timer_ocintpara.outputstate = TIMER_CCX_ENABLE;
timer_ocintpara.ocnpolarity = TIMER_OCN_POLARITY_HIGH;
timer_ocintpara.outputnstate = TIMER_CCXN_DISABLE;
timer_ocintpara.ocidlestate = TIMER_OC_IDLE_STATE_LOW;
timer_ocintpara.ocnidlestate = TIMER_OCN_IDLE_STATE_LOW;
for(i = 0; i < 4; i++) {
timer_channel_output_config(TIMER1,i,&timer_ocintpara);
timer_channel_output_pulse_value_config(TIMER1,i,0);
timer_channel_output_mode_config(TIMER1,i,TIMER_OC_MODE_PWM0);
timer_channel_output_shadow_config(TIMER1,i,TIMER_OC_SHADOW_DISABLE);
}
for(i = 0; i < 3; i++) {
timer_channel_output_config(TIMER2,i,&timer_ocintpara);
timer_channel_output_pulse_value_config(TIMER2,i,0);
timer_channel_output_mode_config(TIMER2,i,TIMER_OC_MODE_PWM0);
timer_channel_output_shadow_config(TIMER2,i,TIMER_OC_SHADOW_DISABLE);
}
/* auto-reload preload enable */
timer_auto_reload_shadow_enable(TIMER1);
timer_auto_reload_shadow_enable(TIMER2);
/* TIMER enable */
timer_enable(TIMER1);
timer_enable(TIMER2);
}
- 完成初始化介面后，剩下的內容就是對輸出特定電機或舵機的pwm進行占空比調節，將調節占空比這個操作封裝成通用的介面，以四輪的電機為例，car_moving這一介面實作的操作就是根據傳入的方向和速度引數去控制電機的正負極電平和占空比，繼而控制車輪轉動的方向和轉速：
void car_moving(MOVING_DIRECTION direction, uint16_t speed_value)
{
uint8_t i;
switch(direction) {
case forward:
gpio_bit_set(GPIOD, MOTOR_PIN_1_P | MOTOR_PIN_2_P | MOTOR_PIN_3_P | MOTOR_PIN_4_P);
gpio_bit_reset(GPIOD, MOTOR_PIN_1_N | MOTOR_PIN_2_N | MOTOR_PIN_3_N | MOTOR_PIN_4_N);
for(i = 0; i < 4; i++) {
timer_channel_output_pulse_value_config(TIMER1,i,speed_value);
}
break;
case right:
gpio_bit_set(GPIOD, MOTOR_PIN_1_P | MOTOR_PIN_2_P | MOTOR_PIN_3_P | MOTOR_PIN_4_P);
gpio_bit_reset(GPIOD, MOTOR_PIN_1_N | MOTOR_PIN_2_N | MOTOR_PIN_3_N | MOTOR_PIN_4_N);
//Since the two sets of wheels on the right are always faster than the left in the actual commissioning process, 60 is added to compensate
timer_channel_output_pulse_value_config(TIMER1,MOTOR_CHANNEL_1,speed_value + 60);
timer_channel_output_pulse_value_config(TIMER1,MOTOR_CHANNEL_3,speed_value + 60);
timer_channel_output_pulse_value_config(TIMER1,MOTOR_CHANNEL_2,speed_value);
timer_channel_output_pulse_value_config(TIMER1,MOTOR_CHANNEL_4,speed_value);
break;
case left:
gpio_bit_set(GPIOD, MOTOR_PIN_1_P | MOTOR_PIN_2_P | MOTOR_PIN_3_P | MOTOR_PIN_4_P);
gpio_bit_reset(GPIOD, MOTOR_PIN_1_N | MOTOR_PIN_2_N | MOTOR_PIN_3_N | MOTOR_PIN_4_N);
timer_channel_output_pulse_value_config(TIMER1,MOTOR_CHANNEL_1,speed_value);
timer_channel_output_pulse_value_config(TIMER1,MOTOR_CHANNEL_3,speed_value);
timer_channel_output_pulse_value_config(TIMER1,MOTOR_CHANNEL_2,speed_value + 50);
timer_channel_output_pulse_value_config(TIMER1,MOTOR_CHANNEL_4,speed_value + 50);
break;
case backward:
gpio_bit_reset(GPIOD, MOTOR_PIN_1_P | MOTOR_PIN_2_P | MOTOR_PIN_3_P | MOTOR_PIN_4_P);
gpio_bit_set(GPIOD, MOTOR_PIN_1_N | MOTOR_PIN_2_N | MOTOR_PIN_3_N | MOTOR_PIN_4_N);
for(i = 0; i < 4; i++) {
timer_channel_output_pulse_value_config(TIMER1,i,speed_value);
}
break;
case stop:
gpio_bit_reset(GPIOD, MOTOR_PIN_1_P | MOTOR_PIN_2_P | MOTOR_PIN_3_P | MOTOR_PIN_4_P);
gpio_bit_reset(GPIOD, MOTOR_PIN_1_N | MOTOR_PIN_2_N | MOTOR_PIN_3_N | MOTOR_PIN_4_N);
for(i = 0; i < 4; i++) {
timer_channel_output_pulse_value_config(TIMER1,i,speed_value);
}
break;
default:
break;
}
}
2.PID控制實作直線行駛
由于本demo方案的四個輪子都是由獨立電機控制的，在給定相同占空比的pwm輸出時，電機與電機之間的誤差以及其他因素都會導致小車并不能按照預想的那樣駛出一條直線，因此我們有必要引入PID演算法來動態調整各個輪子的實際轉速，
理想中的直線行駛，即代表四個輪子轉動了同樣的距離，同時因為這四個輪子的直徑都是相同的，也就相當于要求轉速是一致的，而電機的轉速又直接影響了電機的編碼器輸出脈沖數，因此一個簡單的PID控制思路就產生了：實時采集單位時間下電機編碼器輸出的脈沖數作為PID演算法的輸入，占空比增量作為被控項，通過不斷的反饋調整最終使四個電機的單位時間脈沖數都收斂至同一期望值，
- 首先需要采集四路電機編碼器單位時間脈沖增量,這里是簡單的通過外部中斷計數脈沖的方式來實作（有關外部中斷的配置都放在movement.c檔案中的movement_system_init()函式內）：
void movement_system_init(void)
{
rcu_periph_clock_enable(RCU_SYSCFG);
gpio_mode_set(GPIOC, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_0);
nvic_irq_enable(EXTI0_IRQn, 2U, 0U);
syscfg_exti_line_config(EXTI_SOURCE_GPIOC, EXTI_SOURCE_PIN0);
exti_init(EXTI_0, EXTI_INTERRUPT, EXTI_TRIG_RISING);
exti_interrupt_flag_clear(EXTI_0);
gpio_mode_set(GPIOC, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_1);
nvic_irq_enable(EXTI1_IRQn, 2U, 0U);
syscfg_exti_line_config(EXTI_SOURCE_GPIOC, EXTI_SOURCE_PIN1);
exti_init(EXTI_1, EXTI_INTERRUPT, EXTI_TRIG_RISING);
exti_interrupt_flag_clear(EXTI_1);
gpio_mode_set(GPIOC, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_2);
nvic_irq_enable(EXTI2_IRQn, 2U, 0U);
syscfg_exti_line_config(EXTI_SOURCE_GPIOC, EXTI_SOURCE_PIN2);
exti_init(EXTI_2, EXTI_INTERRUPT, EXTI_TRIG_RISING);
exti_interrupt_flag_clear(EXTI_2);
gpio_mode_set(GPIOC, GPIO_MODE_INPUT, GPIO_PUPD_NONE, GPIO_PIN_3);
nvic_irq_enable(EXTI3_IRQn, 2U, 0U);
syscfg_exti_line_config(EXTI_SOURCE_GPIOC, EXTI_SOURCE_PIN3);
exti_init(EXTI_3, EXTI_INTERRUPT, EXTI_TRIG_RISING);
exti_interrupt_flag_clear(EXTI_3);
}
void EXTI0_IRQHandler(void)
{
if(RESET != exti_interrupt_flag_get(EXTI_0)){
move_state.encoder_pulse[0]++;
}
exti_interrupt_flag_clear(EXTI_0);
}
void EXTI1_IRQHandler(void)
{
if(RESET != exti_interrupt_flag_get(EXTI_1)){
move_state.encoder_pulse[1]++;
}
exti_interrupt_flag_clear(EXTI_1);
}
void EXTI2_IRQHandler(void)
{
if(RESET != exti_interrupt_flag_get(EXTI_2)){
move_state.encoder_pulse[2]++;
}
exti_interrupt_flag_clear(EXTI_2);
}
void EXTI3_IRQHandler(void)
{
if(RESET != exti_interrupt_flag_get(EXTI_3)){
move_state.encoder_pulse[3]++;
}
exti_interrupt_flag_clear(EXTI_3);
}
- 實作forward_correction()介面，該介面將采集到的脈沖數pulse_num、上一次采集的脈沖數pulse_num_old和期望的脈沖增量standard_increment作為引數傳入PID控制函式，根據計算結果呼叫single_motor_speed_set()調整對應單個電機的PWM占空比：
int e[4]={0},e1[4]={0},e2[4]={0}; //pid 偏差
float uk[4]={0.0},uk1[4]={0.0},duk[4]={0.0}; //pid輸出
float Kp=0.8,Ki=0.3,Kd=0.9; //pid控制系數
int out[4] = {0};
static void forward_correction(uint32_t *pulse_num, uint32_t *pulse_num_old, uint32_t standard_increment)
{
uint8_t i = 0;
for(i = 0;i < 4;i++) {
e[i] = standard_increment - (pulse_num[i] - pulse_num_old[i]);
duk[i] = (Kp*(e[i]-e1[i])+Ki*e[i])/100;
uk[i] = uk1[i] + duk[i];
out[i] = (int)uk[i];
uk1[i] = uk[i];
e2[i] = e1[i];
e1[i] = e[i];
single_motor_speed_set(i, (uint16_t)(200+(out[i]*5)));
}
return;
}
- 在運動邏輯輪詢處理函式中，當進入直線狀態的switch分支時，呼叫上面提到的forward_correction()介面來調整電機轉速，然后再把當前采集到的脈沖數encoder_pulse賦值給encoder_pulse_old（todo_judge()將進行轉彎需求判斷，會在后文講到）：
void movement_logic_handle(void)
{
MOVE_STATE_T *p = NULL;
p = &move_state;
uint8_t i = 0;
MOVING_DIRECTION turning_state;
switch(p->todo_type) {
......
case on_the_move:
if(forward_sampling_time_flag == 20) { //20*20ms = 400ms
for(i = 0;i < 4;i++) {
pulse_test[i] = p->encoder_pulse[i]-p->encoder_pulse_old[i];
}
forward_correction(p->encoder_pulse,p->encoder_pulse_old,390);
for(i = 0;i < 4;i++) {
p->encoder_pulse_old[i] = p->encoder_pulse[i];
}
forward_sampling_time_flag = 0;
}
forward_sampling_time_flag++;
todo_judge();
break;
......
default:
break;
}
}
3.實作直角轉彎
以弓字模式為例，當小車直線行駛到規定面積長度時就需要進行直角轉彎動作，在上一節的例程中出現的todo_judge函式就是用于轉彎判斷的，判斷依據也是根據電機編碼器的脈沖數來轉換的，將長度換算成編碼器脈沖數，當脈沖數達到規定長度轉換出的脈沖數時就開始轉彎動作，同時，小車也需要區分當前是在長邊轉彎還是在寬邊轉彎，是往左轉還是往右轉，todo_judge()函式中有一個swich判斷，case分支就是代表小車當前是將要在長邊左右轉還是在寬邊左右轉，case內的代碼主要是判斷當前直線行駛距離是否達到轉彎條件，若達到則改變小車的直行轉彎狀態（change_to_do(turning);），然后改變下一個行駛階段進入該函式時的case分支（p->run_step_flag = width_right;），最后通過寬度是否達到設定值來判斷整個弓字行駛是否結束：
static void todo_judge(void)
{
MOVE_STATE_T *p = NULL;
p = &move_state;
switch(p->run_step_flag) {
case length_right:
if(pulse_to_distance(p->encoder_pulse[0]) >= (p->range_length_mm - 10)) {
if((p->current_angle + 900) > 3600) {
p->target_angle = p->current_angle + 900 - 3600;
}else{
p->target_angle = p->current_angle + 900;
}
change_to_do(turning);
p->run_step_flag = width_right;
}
break;
case width_right:
if(pulse_to_distance(p->encoder_pulse[0]) >= 100) {
if((p->current_angle + 900) > 3600) {
p->target_angle = p->current_angle + 900 - 3600;
}else{
p->target_angle = p->current_angle + 900;
}
change_to_do(turning);
p->run_step_flag = length_left;
width_remain_mm -= 100;
}
break;
case length_left:
if(pulse_to_distance(p->encoder_pulse[0]) >= (p->range_length_mm - 10)) {
if(p->current_angle < 900) {
p->target_angle = 3600 - (900 - p->current_angle);
}else{
p->target_angle = p->current_angle - 900;
}
change_to_do(turning);
p->run_step_flag = width_left;
}
break;
case width_left:
if(pulse_to_distance(p->encoder_pulse[0]) >= 100) {
if(p->current_angle < 900) {
p->target_angle = 3600 - (900 - p->current_angle);
}else{
p->target_angle = p->current_angle - 900;
}
change_to_do(turning);
p->run_step_flag = length_right;
width_remain_mm -= 100;
}
break;
default:
break;
}
if(width_remain_mm <= 0) {
change_to_do(to_stop);
move_state.bow_mode_switch = pdFALSE;
}
}
上面例程中的p->target_angle為目標方位角，p->current_angle為當前方位角，用于控制小車的轉彎角度，目標方位角是由即將轉彎時的小車當前方位角加減90度得來的，而當前方位角則是由地磁傳感器資料計算得出，傳感器型號為QMC5883L,是IIC驅動的，具體驅動代碼都在QMC5883L.c檔案里，這里就不在贅述，主要用到的傳感器介面就是QMC5883L_GetAngle(),獲取當前方位角：
void move_control_task(void *pvParameters)
{
float_xyz Mag_angle;
uint8_t test_flag = 50;
vTaskDelay(200);
QMC_Init();
QMC5883L_GetAngle(&Mag_angle);
move_state.current_angle = (int16_t)Mag_angle.x;
vTaskDelay(500);
while(1) {
if(test_flag){
vTaskDelay(20);
test_flag--;
}else if(test_flag == 0) {
vTaskDelay(20);
movement_logic_handle();
}
QMC5883L_GetAngle(&Mag_angle);
move_state.current_angle = (int16_t)Mag_angle.x;
}
}
得到了方位角，就可以用在轉彎程序中的判斷了，這里實作了一個angle_correction()介面，用于得出當前小車是需要繼續左轉或者右轉還是已經轉到目標角度可以直行了，和直行一樣，angle_correction()介面在運動邏輯輪詢處理函式中的case分支turning中呼叫：
static MOVING_DIRECTION angle_correction(void)
{
int16_t target,current = 0;
target = move_state.target_angle;
current = move_state.current_angle;
if(target < current) {
if(current - target <= 20) {
return forward;
}
if(current - target <= 1800) {
return left;
}else{
return right;
}
}else if(target > current) {
if(target - current <= 20) {
return forward;
}
if(target - current <= 1800) {
return right;
}else{
return left;
}
}else if(current == target) {
return forward;
}
}
void movement_logic_handle(void)
{
MOVE_STATE_T *p = NULL;
p = &move_state;
uint8_t i = 0;
MOVING_DIRECTION turning_state;
switch(p->todo_type) {
......
case on_the_move:
if(forward_sampling_time_flag == 20) { //20*20ms = 400ms
for(i = 0;i < 4;i++) {
pulse_test[i] = p->encoder_pulse[i]-p->encoder_pulse_old[i];
}
forward_correction(p->encoder_pulse,p->encoder_pulse_old,390);
for(i = 0;i < 4;i++) {
p->encoder_pulse_old[i] = p->encoder_pulse[i];
}
forward_sampling_time_flag = 0;
}
forward_sampling_time_flag++;
todo_judge();
break;
case turning:
turning_state = angle_correction();
if(turning_state == right) {
car_full_turn(right,150);
turning_sampling_time_flag = 0;
}else if(turning_state == left) {
car_full_turn(left,150);
turning_sampling_time_flag = 0;
}else if(turning_state == forward) {
car_moving(stop,0);
turning_sampling_time_flag++;
}
if(turning_sampling_time_flag >= 25) {
p->todo_type = on_the_move;
car_moving(forward,200);
forward_sampling_time_flag = 0;
turning_sampling_time_flag = 0;
for(i = 0;i < 4;i++) {
p->encoder_pulse[i] = 0;
p->encoder_pulse_old[i] = 0;
}
}
break;
......
default:
break;
}
}
4.刀片位置與刀片速度設定
刀片位置是由兩個舵機一起控制的，在位置上下變動的同時還需要保持刀片的水平姿態不變，這就需要兩個舵機在相反的方向變動相同的角度，這里直接封裝出一個刀片位置設定介面，入參為位置列舉值：
void blade_position_set(BLADE_POSITION value)
{
switch(value) {
case low:
timer_channel_output_pulse_value_config(TIMER2,TIMER_CH_0,3100);
timer_channel_output_pulse_value_config(TIMER2,TIMER_CH_1,3000);
break;
case medium:
timer_channel_output_pulse_value_config(TIMER2,TIMER_CH_0,2800);
timer_channel_output_pulse_value_config(TIMER2,TIMER_CH_1,2700);
break;
case high:
timer_channel_output_pulse_value_config(TIMER2,TIMER_CH_0,2600);
timer_channel_output_pulse_value_config(TIMER2,TIMER_CH_1,2500);
break;
default:
break;
}
}
刀片速度則是由電調控制的,封裝介面為blade_speed_set();
void blade_speed_set(BLADE_SPEED speed)
{
switch(speed) {
case init:
timer_channel_output_pulse_value_config(TIMER2,TIMER_CH_2,1500);
break;
case off:
timer_channel_output_pulse_value_config(TIMER2,TIMER_CH_2,0);
break;
case low_speed:
timer_channel_output_pulse_value_config(TIMER2,TIMER_CH_2,1800);
break;
case medium_speed:
timer_channel_output_pulse_value_config(TIMER2,TIMER_CH_2,1900);
break;
case high_speed:
timer_channel_output_pulse_value_config(TIMER2,TIMER_CH_2,2000);
break;
default:
break;
}
}
5.GNSS資料接收
本demo采用的涂鴉GUC300模組是通過串口通信的方式給MCU發送GNSS資料的，這里我們使用MCU的usart1來接收資料,usart0已經被作為與wifi模組通信的串口，
- 首先是串口初始化：
void uart_init(void)
{
/* USART interrupt configuration */
nvic_irq_enable(USART0_IRQn, 0, 0);
nvic_irq_enable(USART1_IRQn, 1, 1);
/* enable USART clock */
rcu_periph_clock_enable(RCU_USART0);
/* connect port to USART0_Tx */
gpio_af_set(GPIOA, GPIO_AF_7, GPIO_PIN_9);
/* connect port to USART0_Rx */
gpio_af_set(GPIOA, GPIO_AF_7, GPIO_PIN_10);
/* configure USART Tx as alternate function push-pull */
gpio_mode_set(GPIOA, GPIO_MODE_AF, GPIO_PUPD_PULLUP,GPIO_PIN_9);
gpio_output_options_set(GPIOA, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ,GPIO_PIN_9);
/* configure USART Rx as alternate function push-pull */
gpio_mode_set(GPIOA, GPIO_MODE_AF, GPIO_PUPD_PULLUP,GPIO_PIN_10);
gpio_output_options_set(GPIOA, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ,GPIO_PIN_10);
/* USART configure */
usart_deinit(USART0);
usart_baudrate_set(USART0,115200U);
usart_receive_config(USART0, USART_RECEIVE_ENABLE);
usart_transmit_config(USART0, USART_TRANSMIT_ENABLE);
usart_enable(USART0);
/* enable USART clock */
rcu_periph_clock_enable(RCU_USART1);
/* connect port to USART0_Tx */
gpio_af_set(GPIOD, GPIO_AF_7, GPIO_PIN_5);
/* connect port to USART0_Rx */
gpio_af_set(GPIOD, GPIO_AF_7, GPIO_PIN_6);
/* configure USART Tx as alternate function push-pull */
gpio_mode_set(GPIOD, GPIO_MODE_AF, GPIO_PUPD_PULLUP,GPIO_PIN_5);
gpio_output_options_set(GPIOD, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ,GPIO_PIN_5);
/* configure USART Rx as alternate function push-pull */
gpio_mode_set(GPIOD, GPIO_MODE_AF, GPIO_PUPD_PULLUP,GPIO_PIN_6);
gpio_output_options_set(GPIOD, GPIO_OTYPE_PP, GPIO_OSPEED_50MHZ,GPIO_PIN_6);
/* USART configure */
usart_deinit(USART1);
usart_baudrate_set(USART1,9600U);
usart_receive_config(USART1, USART_RECEIVE_ENABLE);
usart_transmit_config(USART1, USART_TRANSMIT_ENABLE);
usart_enable(USART1);
/* enable USART0 receive interrupt */
usart_interrupt_enable(USART0, USART_INT_RBNE);
/* enable USART1 receive interrupt */
usart_interrupt_enable(USART1, USART_INT_RBNE);
}
- 然后是串口接收中斷服務函式，涂鴉GPS模組會每隔1秒輸出GNRMC GNGGA GPGSV 等陳述句，我們只需要決議其中的
G
P
G
G
A
即
可
，
GPGGA即可，
GPGGA即可，GPGGA 陳述句包括17個欄位：陳述句標識頭，世界時間，緯度，緯度半球，經度，經度半球，定位質量指示，使用衛星數量，HDOP-水平精度因子，橢球高，高度單位，大地水準面高度例外差值，高度單位，差分GPS資料期限，差分參考基站標號，校驗和結束標記(用回車符CR和換行符LF)，分別用14個逗號進行分隔，
格式示例：$GPGGA,014434.70,3817.13334637,N,12139.72994196,E,4,07,1.5,6.571,M,8.942,M,0.7,0016*79
按照上述格式對串口資料進行處理：
#define USART_FH_len 6 //幀頭長度
char USART_FH[USART_FH_len]={'$','G','N','G','G','A'}; //幀頭
#define USART_FT_len 2 //幀尾長度
uint8_t USART_FT[USART_FT_len]={0X0D,0X0A}; //幀尾
uint8_t data_buf[128] = {0};
uint8_t rx_buf[128] = {0};
uint8_t buff_len = 0;
void USART1_IRQHandler(void)
{
if((RESET != usart_interrupt_flag_get(USART1, USART_INT_FLAG_RBNE)) &&
(RESET != usart_flag_get(USART1, USART_FLAG_RBNE))){
rx_buf[buff_len++] = (uint8_t)usart_data_receive(USART1);
if(rx_buf[0] != USART_FH[0]) {
rx_buf[0] = 0;
buff_len = 0;
}
if(buff_len >= USART_FH_len) {
if(strncmp((char*)rx_buf,USART_FH,USART_FH_len) == 0) {
if(strncmp(&rx_buf[buff_len-2],(char*)USART_FT,USART_FT_len) == 0) {
memcpy(data_buf,rx_buf,buff_len);
memset(rx_buf,0,buff_len);
buff_len = 0;
}
}else {
memset(rx_buf,0,USART_FH_len);
buff_len = 0;
}
}
}
}
void gps_data_task(void *pvParameters)
{
MAP_POINT_t *p;
p = &map_point[point_1];
uint8_t data_len = 0;
while(1) {
vTaskDelay(100);
data_len = strlen((char*)data_buf);
if(data_len != 0){
gps_data_pick(point_1, data_buf, data_len);
memset(data_buf,0,data_len);
}
}
}
.小結
? 至此智能割草機Demo就完成了，在這款智能割草機的基礎上還有很多功能可以深入開發，使體驗更加人性化，智能化，同時您可以基于涂鴉 IoT 平臺豐富它的功能，也可以更加方便的搭建更多智能產品原型，加速智能產品的開發流程，
轉載請註明出處，本文鏈接：https://www.uj5u.com/qita/291119.html
標籤：其他