風力擺？這是不是太簡單了點

 

關於風力擺

風力擺是2015年的電子設計大賽題目

關於風力擺的說明啊，實現的功能啊我就不多說了

網上有很多的資料，說難了這是PID調節什么的，但是說簡單了這就是一個單擺 和 圓錐擺模型

如果你用PDI一點一點調，你會調死，要是用我的方法，呵呵，往下看吧

廢話我就不多說了直接進入正題

所需材料

 材料就簡單了

• 單片機最小系統（最好用stm32 快，開源，資料多）
• 電機驅動模塊2個雙通道的（淘寶上很多 幾塊錢）
• 液晶顯示屏（OLED足夠用了）
• JLink ( 下載程序用的，為了節約成本用串口下載也可以 )
• 碳桿60cm
• 萬向節
• 陀螺儀（mpu6050）
• 空心杯電機（4個）
• 導線若干
• 還有就是電池

先來個圖片吧

 

 

 雖然有點抽，但完全不影響功能

風力擺思路

1. 畫直線就是單擺思路

　　陀螺儀可以測得角加速度，同時經過四元數換算可以得到三軸方向的角度，有了這個基礎就簡單了。

　　

 

　　1.1首先先弄出0°方向的直線

　　　　我選的是roll作為0°軸，這樣可以先用此時角度算出此時應該要的角速度，然后寫兩個環，角度負反饋環 和 角速度負反饋環，就可以平衡了，

　　1.2直線長度可設置

　　　　直線長度就和頂點到地面距離L，期望擺起的角度有關

　　1.3各個方向

　　　　然后通過希望的方向角度，可以算出，x, y （x,y 可以根據自己的喜好設置） 方向上所期望的角度和角速度（這個角度滿足公式 tan（a）= y / x ， 不清楚的慢慢推，中學物理），就可以向180度延申。

　　1.4停止就只要吧期望線長設為0

2.畫圓的思路

　　和畫直線是一樣的，由圓錐擺的性質，可得，擺長一定，擺起的高度和角速度成比例關系，

　　要注意的是這里的角速度和mpu6050的角速度不等價，這里mpu6050測出的角速度是旋轉時的線速度。這個地方不懂的可以多想想。（我可是吃了虧的，白白延遲了半天的進度）

　　這里也可以向上面那樣加兩個負反饋環，不過我就用了一個角速度環，簡單嘛。

然后就沒有然后，這樣就搞定了，附上控制代碼吧

#include "mymath.h"
#include "math.h"
#include "IMU6050.h"
#include "control.h"

//================================================單擺
//得到此時的偏差值
float Get_Offset(Imu_t* imu_t, ALL_Expect* expect, ALL_OFFSET* offset)
{
    float expect_x, expect_y;
    float speed;
    float speed_x, speed_y;
    int dir_x = 0, dir_y = 0;
    
    dir_x = Get_Direct(expect->angle, imu_t->gyro.x, imu_t->roll, imu_t->pitch, 0);
    dir_y = Get_Direct(expect->angle, imu_t->gyro.y, imu_t->roll, imu_t->pitch, 1);
    Get_Expect_X_Y(expect->angle, expect->length_to_angle, &expect_x, &expect_y);
    
    speed = Get_Speed(expect->length_to_angle, imu_t->roll, imu_t->pitch, 60);
    Get_Expect_X_Y(expect->angle, speed, &speed_x, &speed_y);
    
    if(speed>0)
    {
        if((imu_t->gyro.x)*(imu_t->roll)>0)
        {
            offset->x_offset = MY_ABS(expect_x - MY_ABS(imu_t->roll))*dir_x*1.0;
            offset->y_offset = MY_ABS(expect_y - MY_ABS(imu_t->pitch))*dir_y*1.0;
        }
        else
        {
            offset->x_offset = 0;
            offset->y_offset = 0;
        }
    }
    else if(speed < 0)
    {
        
        offset->x_offset = -MY_ABS(expect_x - MY_ABS(imu_t->roll))*dir_x*1.0;
        
        offset->y_offset = -MY_ABS(expect_y - MY_ABS(imu_t->pitch))*dir_y*1.0;    
    }
    else
    {
            offset->x_offset = 0;
            offset->y_offset = 0;
    }
    
    if(speed>=0)
    {
            offset->x_gyro_offset = (speed_x - MY_ABS(imu_t->gyro.x))*dir_x*1.0;
            offset->y_gyro_offset = (speed_y - MY_ABS(imu_t->gyro.y))*dir_y*1.0;
    }
    else if(speed < 0)
    {
        offset->x_gyro_offset = -MY_ABS(imu_t->gyro.x)*dir_x*1.0;
        offset->y_gyro_offset = -MY_ABS(imu_t->gyro.y)*dir_y*1.0;
    }
    
    return speed_y;
}

//得到期望角的方向
int Get_Direct(int angle, float gyro, float roll, float pitch, int x_y)
{
    int speed = 0;
    
    speed = gyro*20;
    if(speed>0)
    {
        return 1;
    }
    else if(speed == 0)
    {
        if(MY_ABS(roll)<3 && MY_ABS(pitch)<3)  //判讀是否沒有動
        {
            if(x_y == 0)
            {
                if(angle<=90)
                {
                    return 1;
                }
                else
                {
                    return -1;
                }
            }
            else if(x_y == 1)
            {
                return 1;
            }
        }
        return 0;
    }
    else if(speed<0)
    {
        return -1;
    }
}

//得到期望的x, y 比例
void Get_Expect_X_Y(int angle, float self, float *x, float* y)
{
    float p=0;
    
    if(self<0)
    {
        *x = 0;
        *y = 0;
        return;
    }
    
    if(angle<90)
    {
        p = tan(ANGLE_TO_RADIAN(angle))*1.0;   //得到比例值
    }
    else if(angle == 90)
    {
        *x = 0;
        *y = self;
        return ;
    }
    else if(angle > 90)
    {
        p = tan(ANGLE_TO_RADIAN(angle-90))*1.0;   //得到比例值
    }
    *x = sqrt(self*self / (1+p*p))*1.0;
    *y = *x * p;
}

//得到一定角度時的速度
//L是桿子的長度
float Get_Speed(float length_to_angle, float roll, float pitch, int L)
{
    float now_angle;
    float h;

    now_angle = sqrt((roll*roll)+(pitch*pitch));
    if(now_angle>length_to_angle)
    {    
        return length_to_angle - now_angle;
    }
    h = L*(cos(ANGLE_TO_RADIAN(now_angle))-cos(ANGLE_TO_RADIAN(length_to_angle)));
    return (sqrt(2*980*h))/L*1.0;
}
    

//===============================================圓錐擺
float Get_Circulat_Offset(Imu_t *imu_t, ALL_Expect* expect, ALL_OFFSET* offset)
{
    float r, speed, a;
    float speed_x, speed_y;
    float angle;
    int dir_x = 0, dir_y = 0;
    
//    dir_x = Get_Direct(expect->angle, imu_t->gyro.x, imu_t->roll, imu_t->pitch, 0);
//    dir_y = Get_Direct(expect->angle, imu_t->gyro.y, imu_t->roll, imu_t->pitch, 1);
    Get_Circulat_Dir(imu_t->roll, imu_t->pitch, &dir_x, &dir_y, 1);
    
    r = 0.7 * sin(ANGLE_TO_RADIAN(expect->length_to_angle));
    a = 9.8 * tan(ANGLE_TO_RADIAN(expect->length_to_angle));  //向心加速度
    speed = sqrt(r * a)*1.0 + 0.31;

    if (imu_t->roll != 0 | imu_t->roll != 180)
        angle = atan2(MY_ABS(imu_t->pitch), MY_ABS(imu_t->roll)) * 57.3;
    else
        angle = 90;
    Get_Expect_X_Y(angle, speed, &speed_y, &speed_x);
        
    offset->x_offset = 0;
    offset->y_offset = 0;
    offset->x_gyro_offset = (speed_x - MY_ABS(imu_t->gyro.x))*dir_x*1.0;
    offset->y_gyro_offset = (speed_y - MY_ABS(imu_t->gyro.y))*dir_y*1.0;
    return speed;;
}

//根據 旋轉方向得到 電機方向
void Get_Circulat_Dir(float roll, float pitch, int *dir_x, int *dir_y, int dir)  // 
{

    int clock_dir[2][4][2] = {
        {
            {+1,-1},  //一相線
            {+1,+1},    //二相線
            {-1,+1},  //三相線
            {-1,-1},  //四相線
        },
        {
            {-1,+1},  //一相線
            {-1,-1},    //二相線
            {+1,-1},  //三相線
            {+1,+1},  //四相線
        }
    }
    ;
    //判斷象限
    if(roll>=0 && pitch>=0)
    {
        *dir_x = clock_dir[dir][0][0];
        *dir_y = clock_dir[dir][0][1];
    }
    else    if(roll<0 && pitch>=0)
    {
        *dir_x = clock_dir[dir][1][0];
        *dir_y = clock_dir[dir][1][1];
    }
    else    if(roll<0 && pitch<0)
    {
        *dir_x = clock_dir[dir][2][0];
        *dir_y = clock_dir[dir][2][1];
    }
    else    if(roll>=0 && pitch<0)
    {
        *dir_x = clock_dir[dir][3][0];
        *dir_y = clock_dir[dir][3][1];
    }
}

也就簡單的兩百行

要不你再看看我的PID調節代碼，看看簡單不簡單？？？

int P1_1=1, P1_2=1, P1_3=1, P1_4=1;
int P2_1=50, P2_2=50, P2_3=50, P2_4=50;
void PID_Judge(ALL_OFFSET offset)
{
    uint16_t pwm1=0, pwm2=0, pwm3=0, pwm4=0;
    
    if(offset.x_offset>=0) 
    {
        pwm1 += P1_1*offset.x_offset;
    }
    else
    {
        pwm2 += P1_2*(-offset.x_offset);
    }
    /////
    if(offset.x_gyro_offset>=0)
    {
        pwm1 += P2_1*offset.x_gyro_offset;
    }
    else
    {
        pwm2 += P2_2*(-offset.x_gyro_offset);
    }
    /////
    if(offset.y_offset>=0)
    {
        pwm3 += P1_3*offset.y_offset;
    }
    else
    {
        pwm4 += P1_4*(-offset.y_offset);
    }
    /////
    if(offset.y_gyro_offset>=0)
    {
        pwm3 += P2_3*offset.y_gyro_offset;
    }
    else
    {
        pwm4 += P2_4*(-offset.y_gyro_offset);
    }
    
    pwm1 = LIMIT_MAX(pwm1, 80);
    pwm2 = LIMIT_MAX(pwm2, 80);
    pwm3 = LIMIT_MAX(pwm3, 80);
    pwm4 = LIMIT_MAX(pwm4, 80);
    
    TIM_SetCompare1(TIM10,pwm1);    //修改比較值，修改占空比
    TIM_SetCompare1(TIM11,pwm2);    //修改比較值，修改占空比
    TIM_SetCompare1(TIM13,pwm3);    //修改比較值，修改占空比
    TIM_SetCompare1(TIM14,pwm4);    //修改比較值，修改占空比
}

 

總結

這個題目說起來很難，其實想成物理模型就很簡單了，也就那樣吧

可惜不附上視頻不然肯定秀一波，和大神做的差不了多少，照樣是亂動 5s 內就能恢復

這個東西加搭硬件寫程序只用了2天，因為我不用怎么調PID，全在數學公式。

其實只需要一天半的，但是那個畫圓錐的地方出現了問題， 不是公式出了問題，而是想法錯了，就是我說的注意的地方，白白焦躁了半天，日了狗，哎。

 

有問題的可以留言哈