標(biāo)題: stm32f103舵機(jī)云臺(tái)（附帶mpu6050解算） [打印本頁(yè)]
作者: abz555    時(shí)間: 2023-7-30 16:13
標(biāo)題: stm32f103舵機(jī)云臺(tái)（附帶mpu6050解算）
#include "stm32f10x.h"                  // Device header
#include "Delay.h"
#include "OLED.h"
#include "Serial.h"
#include <stdio.h>
#include <stdlib.h>
#include "Key.h"
#include "sys.h"
#include "LED.h"
#include "Servo.h"
#include "mpu6050.h"  
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h"

unsigned int KeyNum,Mode;
float Ang1,Ang2,AngFlag;
float Angle1=160,Angle2=70;
float Pitch,Roll,Yaw,Pitch_read,Roll_read,Yaw_read;
int16_t Gx,Gy,Gz;

float low_rc(float x,float alpha);

int main(void)
{
        OLED_Init();
        Key_Init();
        Serial_Init();
        Servo_Init();
        OLED_ShowString(1,1,"Roll:");
        OLED_ShowString(2,1,"Yaw:");
        OLED_ShowString(1,12,".");
        OLED_ShowString(2,12,".");
        OLED_ShowString(3,1,"1:");
        OLED_ShowString(4,1,"2:");
        Servo_SetAngle(160,70);

        MPU6050_DMP_Init();
        Delay_ms(2000);
       
        while (1)
        {
                KeyNum=Key_GetNum();
                if(KeyNum==1)Mode=0;
                if(KeyNum==2)Mode=1;
                OLED_ShowSignedNum(1,6,Roll,5);
                OLED_ShowSignedNum(2,6,Yaw,5);
                OLED_ShowNum(1,13,(int)(Roll*10)%10,1);
                OLED_ShowNum(2,13,(int)(Yaw*10)%10,1);
                OLED_ShowNum(3,3,Angle1,3);
                OLED_ShowNum(4,3,Angle2,3);
               
                if(MPU6050_DMP_Get_Data(&Pitch_read,&Roll_read,&Yaw_read)==0)
                {
                        Roll=Roll_read;if(Roll<-50)Roll=-50;if(Roll>50)Roll=50;
                        Yaw=Yaw_read;if(Yaw<-160)Yaw=-160;if(Yaw>20)Yaw=20;
                }
               
                if(Mode==1)
                {
                        Angle1=160+Yaw;
                        Angle2=70+Roll;
               
                        if(Angle1<0){Angle1=0;}
                        if(Angle1>180){Angle1=180;}
                        if(Angle2<0){Angle2=0;}
                        if(Angle2>180){Angle2=180;}
                }
               
                Servo_SetAngle(Angle1,Angle2);
        }
               
               
}
                       
float low_rc(float x,float alpha)
{
        static float y,last_y;
        y=alpha*x+(1-alpha)*last_y;
        last_y=y;
        return y;
}


作者: luqingfen7    時(shí)間: 2024-8-10 10:47
我這就去買(mǎi)配件，做個(gè)陀螺儀控制云臺(tái)出來(lái)




歡迎光臨 (http://www.torrancerestoration.com/bbs/) Powered by Discuz! X3.1