|
|
//main.c
#include <msp430f5529.h>
//#include "extern_variable.h"
#include "mpu6050.h"
#include "moto.h"
#include "RF24L01-fasong.h"
#include <system_lnit.h>
#include <stdlib.h>
#include "eFly_IIC.h"
#include "eFly_MPU6050.h"
#include "eFly_IMU.h"
#include <eFly_schedule.h>
#include <eFly_PID.h>
#include "eFly_RC.h"
#include "eFly_HMC5883.h"
//#include "uart.h"
static unsigned char uart1_tx_data_agm[23];
char *p,Bluetooth[4],Bi=0,Ci=0,ii=0,Fl=0;
//extern float_xyz Q_angle;
//extern int_xyz Acc_data, Gyro_data;
#define Gyro_Gr 0.0002663
uchar DATA[]="wan quan ok";
float fa = 3.68f;
void sendString(unsigned char *p);
void sendChar(char c);
void uartInit(void);
void Shuchu(float f);
volatile uint32_t Systime_cnt_5ms = 0; // time counter
volatile uint8_t Systime_cnt_flag = 0x00; //system time interrupt indicator
/*
* 系 統 初 始 化
*/
void Sys_Init()
{
WDTCTL = WDTPW + WDTHOLD; // Stop WDT
uartInit();
_BIS_SR(GIE);
P1DIR |= BIT2;// interrupts enabled
delayms(1000);
InitSys(); //改了時鐘串口波特率變了
sendChar('k');
InitMPU6050();
eFly_HMC5883_Init();
eFly_IMU_Init();
// BCSCTL2 |=SELM_2+SELS; //主、從系統時鐘均為高頻
LED_IO_set();
RF24L01_IO_set(); //占用了P3口，我直接刪掉了
init_NRF24L01() ;
eFly_Motor_Init();
eFly_PID_Set_Pitch(9, 9.2, 0.0, 9.00); //(float kp_pit, float kp_q, float ki_q, float kd_q);
eFly_PID_Set_Roll(0, 0, 0.0, 0.00); //(float kp_rol, float kp_p, float ki_p, float kd_p);
eFly_PID_Set_Yaw(0.00, 0.0, 0.00, 0.00); //(float kp_yaw, float kp_r, float ki_r, float kd_r);
// eFly_PID_Set_Altitude(0.0, 0.01, 0.0); //Single PID loop, (float kp_alt, float ki_alt, float kd_alt);
//LED_Flash();
}
extern void ByteWrite(uchar REG_Address,uchar REG_data);
/*
* 主 函 數
*/
void main(void)
{
Sys_Init();
TBCCR0 = 655;
TBCCTL0 = CCIE;
TBCTL = TBSSEL_1 + MC_1 + TBCLR; // ACLK, up mode
sendChar('k');
_EINT();
while(1){
if(Systime_cnt_flag == 0x01)
{
Systime_cnt_flag = 0x00;
// Use prime numbers to take full advantage of the free time of CPU!!!
if(Systime_cnt_5ms % 1 == 0x00) //200Hz
{
P1OUT |= BIT2;
eFly_Loop_200Hz();
P1OUT &= ~BIT2;
//sendChar(eFly_MPU6050_Get_DeviceID());
//DD=eFly_IIC_Read_Bytes(0xD0, 0x3B, 1, DA);
//sendChar(DA[0]);
//sendChar(Acc_Gyro[0]);
//sendChar(Acc_Gyro[0]>>8);
//sendChar(Ctrl_drv_rol);
//sendChar(eFly_MPU6050_Get_DeviceID());
}
if(Systime_cnt_5ms % 2 == 0x00) //100Hz
{
eFly_Loop_100Hz();
}
if(Systime_cnt_5ms % 3 == 0x00) //67Hz
{
eFly_Loop_67Hz();
}
if(Systime_cnt_5ms % 5 == 0x00) //40Hz
{
eFly_Loop_40Hz();
//sendChar(0x55);
//eFly_IIC_Read_Bytes(0xD0, 0x75, 1, dataIIC);
//sendChar(eFly_MPU6050_TestConnection());
//sendChar(0x66);
}
if(Systime_cnt_5ms % 7 == 0x00) //29Hz
{
eFly_Loop_29Hz();
}
if(Systime_cnt_5ms % 11 == 0x00) //18Hz
{
eFly_Loop_18Hz();
}
if(Systime_cnt_5ms % 13 == 0x00) //15Hz
{
eFly_Loop_15Hz();
}
if(Systime_cnt_5ms % 17 == 0x00) //12Hz
{
eFly_Loop_12Hz();
if(ii>=4) {
RC_thr=Bluetooth[0]*1000+Bluetooth[1]*100+Bluetooth[2]*10+100+Bluetooth[3];
if(RC_thr>3000)RC_thr=0;
sendChar(Bluetooth[0]+'0');
sendChar(Bluetooth[1]+'0');
sendChar(Bluetooth[2]+'0');
sendChar(Bluetooth[3]+'0');
Bluetooth[0]=0;
Bluetooth[1]=0;
Bluetooth[2]=0;
Bluetooth[3]=0;
ii=0;
}
}
if(Systime_cnt_5ms % 19 == 0x00) //11Hz
{
eFly_Loop_11Hz();
}
if(Systime_cnt_5ms % 21 == 0x00) //10Hz
{
eFly_Loop_10Hz();/*
volatile float kP_p = 1, kP_q = 1, kP_r = 0, kP_w = 1;
volatile float kI_p = 0, kI_q = 0, kI_r = 0, kI_w = 0;
volatile float kD_p = 0, kD_q = 0, kD_r = 0, kD_w = 0;
volatile float kP_pit = 1, kP_rol = 1, kP_yaw = 1;
volatile float kP_alt = 0, kI_alt = 0, kD_alt = 0;
volatile float kP_pos = 0, kI_pos = 0, kD_pos = 0;*/
//
if(ii<4){
if(Bluetooth[0]==1){kP_pit=kP_pit+0.1;Bluetooth[0]=0;}//Ci++;Bluetooth[0]=0;}
if(Bluetooth[0]==2){kP_pit=kP_pit-0.1;Bluetooth[0]=0;}//Ci--;Bluetooth[0]=0;}
if(Bluetooth[0]==3){kP_q=kP_q+0.1;Bluetooth[0]=0;}//Bi++;Bluetooth[0]=0;}
if(Bluetooth[0]==4){kP_q=kP_q-0.1;Bluetooth[0]=0;}//Bi--;Bluetooth[0]=0;}
if(Bluetooth[0]==5){
kD_q=kD_q+0.1;
/*
UCA0TXBUF=Bi/10%10+'0';
while(!(UCA0IFG&UCTXIFG));
UCA0TXBUF=Bi%10%10+'0';
while(!(UCA0IFG&UCTXIFG));
UCA0TXBUF='F';
while(!(UCA0IFG&UCTXIFG));
UCA0TXBUF=Ci/10%10+'0';
while(!(UCA0IFG&UCTXIFG));
UCA0TXBUF=Ci%10%10+'0';
while(!(UCA0IFG&UCTXIFG));
UCA0TXBUF='F';
while(!(UCA0IFG&UCTXIFG));
Bluetooth[0]=0;*/
}
if(Bluetooth[0]==6){
kD_q=kD_q-0.1;
Bluetooth[0]=0;}
/*
switch(Bi)
{
case 0:break;
case 1:kP_p =kP_p+1.0*Ci/10;Shuchu(kP_p);break;
case 2:kP_q =kP_q+1.0*Ci/10;Shuchu(kP_q);break;
case 3:kP_r =kP_r+1.0*Ci/10;Shuchu(kP_r);break;
case 4:kP_w =kP_w+1.0*Ci/10;Shuchu(kP_w);break;
case 5:kI_p =kI_p+1.0*Ci/10;Shuchu(kI_p);break;
case 6:kI_q =kI_q+1.0*Ci/10;Shuchu(kI_q);break;
case 7:kI_r =kI_r+1.0*Ci/10;Shuchu(kI_r);break;
case 8:kI_w =kI_w+1.0*Ci/10;Shuchu(kI_w);break;
case 9:kD_p =kD_p+1.0*Ci/10;Shuchu(kD_p);break;
case 10:kD_q =kD_q+1.0*Ci/10;Shuchu(kD_q);break;
case 11:kD_r =kD_r+1.0*Ci/10;Shuchu(kD_r);break;
case 12:kD_w =kD_w+1.0*Ci/10;Shuchu(kD_w);break;
case 13:kP_pit =kP_pit+1.0*Ci/10;Shuchu(kP_pit);break;
case 14:kP_rol =kP_rol+1.0*Ci/10;Shuchu(kP_rol);break;
case 15:kP_yaw =kP_yaw+1.0*Ci/10;Shuchu(kP_yaw);break;
case 16:kP_alt =kP_alt+1.0*Ci/10;Shuchu(kP_alt);break;
case 17:kI_alt =kI_alt+1.0*Ci/10;Shuchu(kI_alt);break;
case 18:kD_alt =kD_alt+1.0*Ci/10;Shuchu(kD_alt);break;
case 19:kP_pos =kP_pos+1.0*Ci/10;Shuchu(kP_pos);break;
case 20:kI_pos =kI_pos+1.0*Ci/10;Shuchu(kI_pos);break;
case 21:kD_pos =kD_pos+1.0*Ci/10;Shuchu(kD_pos);break;
default: break;
}
Bi=0;
Ci=0;
}*/
ii=0;
}
}
if(Systime_cnt_5ms % 101 == 0x00) //2Hz
{
eFly_Loop_2Hz();
}
}
else { }
}
}
#pragma vector=TIMERB0_VECTOR
__interrupt void Timer_B (void)
{
Systime_cnt_5ms++;
Systime_cnt_flag = 0x01;
}
// Echo back RXed character, confirm TX buffer is ready first
#pragma vector=USCI_A0_VECTOR
__interrupt void USCI_A0_ISR(void)
{char biao;
switch(__even_in_range(UCA0IV,4))
{
case 0:break; // Vector 0 - no interrupt
case 2: // Vector 2 - RXIFG
while (!(UCA0IFG&UCTXIFG)); // USCI_A0 TX buffer ready?
UCA0TXBUF = UCA0RXBUF; // TX -> RXed character
biao=UCA0RXBUF;
if(biao!='F'){
Bluetooth[ii++]=biao-'0';
}
else ;
break;
case 4:break; // Vector 4 - TXIFG
default: break;
}
}
/****************************************************************************
*函數名: sendChar() /
*作用 ：USART0發送一個字符 /
*返回值：無 /
*參數 ：一個字符 /
*作者 ：Zhenhua Liu /
*時間 ：2017.11.01 /
*****************************************************************************/
void sendChar(char c){
UCA0TXBUF=c;
while(!(UCA0IFG&UCTXIFG));//等待數據發送完畢
}
/****************************************************************************
*函數名: sendString() /
*作用 ：USART0發送字符串 /
*返回值：無 /
*參數 ：char型指針 /
*作者 ：Zhenhua Liu /
*時間 ：2017.11.01 /
*****************************************************************************/
void sendString(unsigned char *p){
while(*p!='\0'){
sendChar(*p++);
}
}
/****************************************************************************
*函數名: uartInit() /
*作用 ：USART0的初始化配置，使用P3.4和P3.5，不使用中斷 /
*返回值：無 /
*參數 ：無 /
*作者 ：Zhenhua Liu /
*時間 ：2017.11.01 /
*****************************************************************************/
void uartInit(){
P3SEL |= BIT3+BIT4; // P3.3,4 = USCI_A0 TXD/RXD
UCA0CTL1 |= UCSWRST; // **Put state machine in reset**
UCA0CTL1 |= UCSSEL_2; // SMCLK
UCA0BR0 = 450; // 1MHz 115200 (see User's Guide) //應為始終改變，所以改變時鐘與波特率的比值 12M/115200
UCA0BR1 = 4; // 1MHz 115200
UCA0MCTL |= UCBRS_1 + UCBRF_0; // Modulation UCBRSx=1, UCBRFx=0
UCA0CTL1 &= ~UCSWRST; // **Initialize USCI state machine**
UCA0IE |= UCRXIE; // Enable USCI_A0 RX interrupt
__bis_SR_register(GIE); // Enter LPM0, interrupts enabled
}
void Shuchu(float f){
int If;
If=(int)(f*10);
UCA0TXBUF=If/100+'0';
while(!(UCA0IFG&UCTXIFG));//等待數據發送完畢
UCA0TXBUF=If%100/10+'0';
while(!(UCA0IFG&UCTXIFG));//等待數據發送完畢
UCA0TXBUF='.';
while(!(UCA0IFG&UCTXIFG));//等待數據發送完畢
UCA0TXBUF=If%10+'0';
while(!(UCA0IFG&UCTXIFG));//等待數據發送完畢
UCA0TXBUF='/n';
while(!(UCA0IFG&UCTXIFG));//等待數據發送完畢
}
|
-
-
44444.rar
2019-6-14 17:14 上傳
點擊文件名下載附件
下載積分: 黑幣 -5
379.9 KB, 下載次數: 9, 下載積分: 黑幣 -5
|
QQ好友和群
QQ空間
騰訊微博
騰訊朋友
收藏
淘帖
頂
踩
