STM32小四轴自用调试代码分享无HMC5883程序 -

#include "sys.h"
#include "nrf24l01.h"
#include "mpu6050.h"
#include "motor.h"
#include "led.h"
#include "nrfreport.h"
#include "flycontro.h"
#include "mpudat.h"
/**
微型四轴代码，2016/1/22,chrain
无HMC5883程序
*/
int U8toInt(u8 a,u8 b);
void FLY_Contro(void);
int main(void)
{
int PWM;
// PWMA =PWMB =PWMC =PWMD =0;
SystemInit();
delay_init();
LED_Init(); //LED初始化
if(NRF24L01_Init())
LED1 = LED2 = LED3 = LED4 = 1;
delay_ms(1000);
LED1 = LED2 = LED3 = LED4 = 1;
MPU_Init(); //陀螺仪初始化
PID_init();
delay_ms(1000);
Motor_Init();//电机初始化
timer_Init();
// MPU6050_offset();
// LED1 = LED2 = LED3 = LED4 = 0;
//
// RxBuf[0]=RxBuf[1]=RxBuf[2]=RxBuf[3]=0;
// PWMA = PWMB = PWMC = PWMD =0;
// PWM_Init();
// PID_init(); //PID参数初始化
while(1)
{
if(RxBuf[0]&0xA0) LED1 = LED2 = LED3 = LED4 = 1;
else LED_Circleflsah ();
if(RxBuf[2]>140|RxBuf[2]<115)
Ycontrdat = (125 - RxBuf[2])/12.0;
else
Ycontrdat =0;
if(RxBuf[1]>130|RxBuf[1]<120)
Xcontrdat = (RxBuf[1] - 125)/12.0;
else
Xcontrdat =0;
TargeContrdat.X = U8toInt(RxBuf[5],RxBuf[6])/100.0;
TargeContrdat.Y = U8toInt(RxBuf[7],RxBuf[8])/100.0;
TargeContrdat.Z = U8toInt(RxBuf[9],RxBuf[10])/100.0;
if(RxBuf[0] == 0XA1)
{
PID_Kp.X = U8toInt(RxBuf[11],RxBuf[12])/100.0;
PID_Kp.Y = U8toInt(RxBuf[13],RxBuf[14])/100.0;
PID_Kp.Z = U8toInt(RxBuf[15],RxBuf[16])/100.0;
PID_Ki.X = U8toInt(RxBuf[17],RxBuf[18])/100.0;
PID_Ki.Y = U8toInt(RxBuf[19],RxBuf[20])/100.0;
PID_Ki.Z = U8toInt(RxBuf[21],RxBuf[22])/100.0;
PID_Kd.X = U8toInt(RxBuf[23],RxBuf[24])/100.0;
PID_Kd.Y = U8toInt(RxBuf[25],RxBuf[26])/100.0;
PID_Kd.Z = U8toInt(RxBuf[27],RxBuf[28])/100.0;
}
delay_ms(20);
// NRF_Report_STATUS(Angle.X*100,Angle.Y*100,Angle.Z*100,Accel.X,Accel.X,Accel.Z,Gyro.X,Gyro.Y,Gyro.Z);
NRF_Report_STATUS(Angle.Y*100,Ycontrdat*100,Angle.Z*100,
Angle.Y*100,Ycontrdat*100,Accel.Z,
Gyro.X,Gyro.Y,Gyro.Z);
PWM = 256 - RxBuf[3];
if(RxBuf[0]&0xA0)
{
if(PWM > 30)
{
if(PWM>250) PWM =249;
Fly_PWM = (PWM-30)*2.2;
}else
{
Fly_PWM =0;
Angle.Z =0;
}
}else
{
Fly_PWM =0;
Angle.Z =0;
}
// Prepare_Data(&Acc,&Acc_AVG);
// IMUupdate(&Gyr,&Acc_AVG,&Angle);
// LED2 =1;
// delay_ms(100);
// LED1=~LED1;
// NRF24l01_Report_STATUS((int)(Angle.rol *100),(int)(Angle.pit *100),(int)(Angle.yaw*100),0,0);
//
//i++;
//if(i>10)
//{
// TxBuf[15] = (Moto_A>>8)&0x00ff;
// TxBuf[16] = Moto_A&0x00ff;
// TxBuf[17] = (Moto_B>>8)&0x00ff;
// TxBuf[18] = Moto_B&0x00ff;
// TxBuf[19] = (Moto_C>>8)&0x00ff;
// TxBuf[20] = Moto_C&0x00ff;
// TxBuf[21] = (Moto_D>>8)&0x00ff;
// TxBuf[22] = Moto_D&0x00ff;
// i=0;
}
// FLY_Contro();
// if(RxBuf[1]>0)
// Fly_PWM = RxBuf[1]*5;
// else
// Fly_PWM=0;
//
//
//
// }
}
void FLY_Contro(void)
{
//float RolContro,PitContro;
//float temp;
//static float Rol_weitiao=0,Pit_weitiao=0;
//
//
//
////PIT控制
// if(RxBuf[5]<115)
// {
// PitContro = (115-RxBuf[5])/5.0;
// if(PitContro >= 20) PitContro=20.0;
// }else if(RxBuf[5]>125) // if(TxBuf[3]<115) //TxBuf[3]>125
// {
// PitContro = (125-RxBuf[5])/5.0;
// if(PitContro<=-20) PitContro=-20;
// }
////ROll控制
// if(RxBuf[4]<115)
// {
// RolContro = (115-RxBuf[4])/5.0;
// if(RolContro >= 20) RolContro=20.0;
// }else if(RxBuf[4]>125) // if(TxBuf[3]<115) //TxBuf[3]>125
// {
// RolContro = (125- RxBuf[4])/5.0;
// if(RolContro<=-20) RolContro=-20;
// }
//
//
// Roll.Targe = RolContro + Rol_weitiao;
// Pitch.Targe = -PitContro + Pit_weitiao;
//
// if(RxBuf[9]==1)
// {
// temp = U8toInt(RxBuf[10],RxBuf[11]); //ROll微调
// // Pit_PID.Kp = temp/100.0;
// Rol_weitiao= temp/4.0;
// temp = U8toInt(RxBuf[12],RxBuf[13]); //PIt微调
// Pit_weitiao= temp/4.0;
//// Pit_PID.Kd= temp/100.0;
// }
// RxBuf[9]=0;
}
int U8toInt(u8 a,u8 b)
{
int temp=0;
// b = temp&0x00ff;
// a = (temp>>8)&0x00ff;
temp = (a<<8)|b;
if(temp>32768)temp-=65536;
return temp;
}
//void PWM_Init(void)
//{
// GPIO_InitTypeDef GPIO_InitStructure;
……………………
…………限于本文篇幅余下代码请从51黑下载附件…………

复制代码