int main(void)
{
CPU_INT08U os_err;
//禁止CPU中断
CPU_IntDis();
//UCOS 初始化
OSInit(); /* Initialize "uC/OS-II, The Real-Time Kernel". */
//硬件平台初始化
BSP_Init(); /* Initialize BSP functions. */
/* Configure FSMC Bank1 NOR/PSRAM */
I2C_Ini();
HMC5883L_Init();
HMC5883L_Start();
MPU6050_Init();
//建立主任务， 优先级最高 建立这个任务另外一个用途是为了以后使用统计任务
os_err = OSTaskCreate((void (*) (void *)) App_TaskStart, //指向任务代码的指针
(void *) 0, //任务开始执行时，传递给任务的参数的指针
(OS_STK *) &App_TaskStartStk[APP_TASK_START_STK_SIZE - 1], //分配给任务的堆栈的栈顶指针 从顶向下递减
(INT8U) APP_TASK_START_PRIO); //分配给任务的优先级
os_err =os_err;
//ucos的节拍计数器清0 节拍计数器是0-4294967295 对于节拍频率100hz时， 每隔497天就重新计数
OSTimeSet(0);
OSStart(); /* Start multitasking (i.e. give control to uC/OS-II). */
/* Start multitasking (i.e. give control to uC/OS-II). */
return (0);
}
int main(void)
{
Periph_clock_enable();
GPIO_Config();
LEDon;
Delay_ms(10); //short blink
LEDoff;
Delay_ms(50);
Usart4Init();
ADC_Config();
MPU6050_Init();
Timer2_Config();
Timer3_Config();//RC control timer
NVIC_Configuration();
EXTI_Config();
//engineInit(); ///????to initialize all variables;
configLoad();
MPU6050_Gyro_calibration();
while(1)
{
engineProcess();
while(stop==0) {}//Closed loop waits for interrupt
}
}
void SYS_INIT(void)
{
LED_INIT();
LED_FLASH();
Moto_Init();
LED_FLASH();
Tim3_Init(500);
Nvic_Init();
Uart1_Init(115200);
ANO_TC_I2C2_INIT(0xA6,400000,1,1,3,3);
MPU6050_Init();
Spi1_Init();
Nrf24l01_Init(MODEL_TX2,40);
if(Nrf24l01_Check())
Uart1_Put_String("NRF24L01 IS OK !\r\n");
else
Uart1_Put_String("NRF24L01 IS NOT OK !\r\n");
ADC1_Init();
FLASH_Unlock();
EE_INIT();
EE_READ_ACC_OFFSET();
EE_READ_GYRO_OFFSET();
EE_READ_PID();
Tim3_Control(1);
}
/*
* JGA25-371全金属电机(12V)、超耐磨65MM橡胶轮胎、3MM铝合金车身
* 2014.02.08 Regis
*/
void vTask_Balance_Car_1(void *pvParameters )
{
(void)pvParameters; //prevent compiler worning/error
char m[100];
portTickType xLastWakeTime;
//portTickType xStartTime, xEndTime;
Balance_Car_Init();
Motor_L298N_Init();
MPU6050_Init();
Motor_L298N_PWM_Init(1000); //1kHz
xLastWakeTime = xTaskGetTickCount();
for( ;; )
{
vTaskDelayUntil(&xLastWakeTime, task_dt); // set the timer as 10ms
BT_Cmd();
//xStartTime = xTaskGetTickCount();
Angle_Calculate();
Position_Calculate();
PWM_Calculate();
speed_mr = speed_ml = 0;
//xEndTime = xTaskGetTickCount();
//xEndTime -= xStartTime;
if (cmd_sw_debug)
{
//sprintf(m, "%u %f %f S:%f %f P:%d %d\r\n", xEndTime, Angle, Gyro_y, speed, position, PWM_R, PWM_L);
sprintf(m, "%f %f %f %d\r\n", Angle, Gyro_y, speed, PWM_L);
UART_PutString(UART_BT, m);
}
}
}
/*====================================================================================================*/
void System_Init( void )
{
MPU_InitTypeDef MPU_InitStruct;
HAL_Init();
GPIO_Config();
Serial_Config();
MPU6050_Config();
Delay_100ms(1);
printf("\r\nHello World!\r\n\r\n");
LED_B_Reset();
MPU_InitStruct.MPU_Gyr_FullScale = MPU_GyrFS_2000dps;
MPU_InitStruct.MPU_Gyr_LowPassFilter = MPU_GyrLPS_41Hz;
MPU_InitStruct.MPU_Acc_FullScale = MPU_AccFS_4g;
MPU_InitStruct.MPU_Acc_LowPassFilter = MPU_AccLPS_41Hz;
while(MPU6050_Init(&MPU_InitStruct) != SUCCESS) {
LED_R_Toggle();
Delay_100ms(1);
}
LED_R_Set();
LED_B_Set();
Delay_100ms(1);
}
void main()
{
WDTCTL = WDTPW + WDTHOLD;
Clock_Init(0,0,0,CPU_FF); // 12M DCO
LCD5110_Init();
LCD5110_Write_String(0,0,LCD_MAIN_BUFFER1);
LCD5110_Write_String(0,1,LCD_MAIN_BUFFER2);
LCD5110_Write_String(0,2,LCD_MAIN_BUFFER3);
LCD5110_Write_String(0,3,LCD_MAIN_BUFFER4);
LCD5110_Write_String(0,4,LCD_MAIN_BUFFER5);
MPU6050_Init();
while(1)
{
MPU6050_AccelX();
LCD5110_Long2Char(MPU6050_AX);
LCD5110_Write_String(18,0,0);
MPU6050_AccelY();
LCD5110_Long2Char(MPU6050_AY);
LCD5110_Write_String(18,1,0);
MPU6050_AccelZ();
LCD5110_Long2Char(MPU6050_AZ);
LCD5110_Write_String(18,2,0);
MPU6050_Angle_XZX();
MPU6050_Angle_YZY();
LCD5110_Long2Char(MPU6050_ANGLE_XZX);
LCD5110_Write_String(18,3,0);
LCD5110_Long2Char(MPU6050_ANGLE_YZY);
LCD5110_Write_String(18,4,0);
/*
MPU6050_AnguX();
LCD5110_Long2Char(MPU6050_GX);
LCD5110_Write_String(18,3,0);
MPU6050_AnguY();
LCD5110_Long2Char(MPU6050_GY);
LCD5110_Write_String(18,4,0);
MPU6050_AnguZ();
LCD5110_Long2Char(MPU6050_GZ);
LCD5110_Write_String(18,5,0);
*/
DELAY_MS(500);
}
}
void iniciarMPU6050Imu() {
MPU6050_InitStruct initialConfig;
//Configurações dos sensores.
initialConfig.accelFullScale = AFS_8G; //Fundo de escala de 8G.
initialConfig.gyroFullScale = FS_2000DPS; //Fundo de escala de 500 graus por segundo.
initialConfig.clockSource = MPU6050_CLK_GYRO_X_PLL; //Fonte de clock no oscilador do eixo X do giroscópio.
initialConfig.fifoEnabled = 0; //Fifo desligada;
initialConfig.sampleRateDivider = 0; //Frequência do Accel é igual à do Gyro //Taxa de saída de 500 Hz
initialConfig.temperatureSensorDisabled = 0; //Sensor de temperatura ligado.
initialConfig.interruptsConfig = 0x01; //Ativa a interrupção de Data Ready;
initialConfig.intPinConfig = 0x20; //Ativa o pino de interrupção com o modo que o "liga" quando há uma interrupção.
initialConfig.digitalLowPassConfig = 0x01; //Sem filtro passa baixa
//initialConfig.digitalLowPassConfig = 0x02; //Frequências de corte em 90Hz e Aquisição em 1Khz. (Delay de aprox 10ms)
//initialConfig.digitalLowPassConfig = 0x03; //Frequências de corte em 40Hz e Aquisição em 1Khz. (Delay de aprox 5ms)
//initialConfig.digitalLowPassConfig = 0x04; //Frequências de corte em 20Hz e Aquisição em 1Khz. (Delay de aprox 8,5ms)
//initialConfig.digitalLowPassConfig = 0x00; //Frequências de corte em 260Hz e Aquisição em 8Khz. (Delay de aprox 0.98ms)
MPU6050_Init(I2C3, &initialConfig);
float offset_accel[3] = {0,0,0};
float offset_gyro[3] = {0,0,0};
float temp_accel[3];
float temp_gyro[3];
delay(10000);
MPU6050_configIntPin(RCC_AHB1Periph_GPIOD, GPIOD, GPIO_Pin_0);
//Carregar valores de "parado" (Offsets).
uint16_t counterOffsetAquisition = 10000;
while(counterOffsetAquisition--) {
while(MPU6050_checkDataReadyIntPin() == Bit_RESET);
MPU6050_readData(I2C3, temp_accel, temp_gyro);
GPIO_ToggleBits(GPIOD, GPIO_Pin_12);
offset_accel[0] += temp_accel[0];
offset_accel[1] += temp_accel[1];
offset_accel[2] += (1-temp_accel[2]);
offset_gyro[0] += temp_gyro[0];
offset_gyro[1] += temp_gyro[1];
offset_gyro[2] += temp_gyro[2];
}
offset_accel[0] = offset_accel[0]/((float)(10000));
offset_accel[1] = offset_accel[1]/((float)(10000));
offset_accel[2] = offset_accel[2]/((float)(10000));
offset_gyro[0] = offset_gyro[0]/((float)(10000));
offset_gyro[1] = offset_gyro[1]/((float)(10000));
offset_gyro[2] = offset_gyro[2]/((float)(10000));
setar_offset_acel(offset_accel);
setar_offset_gyro(offset_gyro);
}
/*
* Name : initialize
* Description : ---
* Author : lynx@sia.
*
* History
* ----------------------
* Rev : 0.00
* Date : 10/20/2013
*
* create.
* ----------------------
*/
static int initialize(void)
{
MPU6050_Init(); //Keep MPU-6050 initialized first
AK8975_initialize(); //Do nothing infact
//MS5611_Init(&imu_sensor.Bar);
imu_sensor.Gyr.RawToTrue = 16.3835f; //2000dps--16.4LSB
imu_sensor.Acc.RawToTrue = 8192.0f; //4g--8192LSB
imu_sensor.Mag.RawToTrue = 3.332f; //1229uT--3.3319772172497965825874694873881LSB
return 0;
}
void init_i2c()
{
uint8 device_id = MPU6050_Init();
//通过设备ID判断设备是否为MPU6050
if(device_id == MPU6050_ID)
{
printf("MPU6050初始化成功!\r\n");
printf("设备ID: 0x%X\r\n", device_id);
}
else
{
printf("MPU6050初始化失败!\r\n");
printf("设备ID: 0x%X\r\n", device_id);
//while(1);
}
}
void InitAll()
{
__disable_irq();
TimInit();
PWMStart();
InputDecoder();
UartInit();
FlashInit();
SDFatFSInit();
printf("mpu6050 id:0x%x\r\n",MPU6050_Init());
IMU_Init();
UIInit();
sys.status = READY;
HAL_ADC_Start(&hadc1);
printf("init finish!\r\n");
__enable_irq();
}
void My_System_Init(void)
{
uint8_t flag, i=0;
NVIC_Configuration();
delay_init(72); //延时初始化
for(i=0;i<5;i++)
delay_Ms_Loop(1000); //上电短延时 确保供电稳定
LED_Init();
LEDALL_ON;
timer2_init(); //PWM输出定时器初始化
timer3_init(); //PWM输出定时器初始化
IIC_Init();
uart_init(38400); //调试用串口初始化
My_usart2_init(38400); //蓝牙用串口初始化
printf("欢迎使用启天科技BUTTERFLY四旋翼\r\n");
printf("QQ群：471023785\r\n");
LEDALL_OFF;
MPU6050_Init(); //6050初始化
SPI1_INIT(); //SPI初始化，用于nRF模块
flag = NRF_CHECK(); //检查NRF模块是否正常工作
if(flag != 1)
{
while(1)
{
LEDALL_OFF;
delay_Ms_Loop(200);
LEDALL_ON;
delay_Ms_Loop(200);
}
}
NRF24L01_INIT(); //nRF初始化
SetRX_Mode(); //设置为接收模式
NRF24L01_INIT(); //nRF初始化
NRF_GPIO_Interrupt_Init(); //nRF使用的外部中断的引脚初始化
tim4_init(); //定时中断，作为系统的控制频率
adcInit(); //ADC初始化，测量电池电压
}
/* ===================================================================*/
void PeripheralInit(void)
{
/* Initialize MPU6050 and HMC5883. */
HMC5883_Init();
MPU6050_Init();
}
/*******************************************************
目前暂时都是开环控制，没有闭环
******************************************************/
int main()
{
float AngleRight = 0, AngleLift = 0;
uint8_t flag = 0;
delay_init(72);
Board_Init();
MPU6050_Init();
PWM_Init();
OLED_Init();
//
//=================================================//
// 初始化.... 风扇全都工作，免去启动长延时等待
Motor_Run(Motor_1, 0.2);
Motor_Run(Motor_2, 0.2);
Motor_Run(Motor_3, 0.2);
Motor_Run(Motor_4, 0.2);
delay_ms(1000);
delay_ms(1000);
Timer3_Init();
//=================================================//
OLED_P8x16Str(0, 0, "Stone");
OLED_P8x16Str(0, 2, "Y");
OLED_P8x16Str(0, 4, "X");
OLED_P8x16Str(0, 6, "pit:");
OLED_P8x16Str(0, 6, "pit: ");
while(1)
{
AnglePendulum_CC(330);
// StopPendulum_Y();
// Motor_Run(Motor_1, 0);
// Motor_Run(Motor_2, 0);
// Motor_Run(Motor_3, 0.5);
// Motor_Run(Motor_4, 0.1);
}
while(1)
{
/***************************测试代码***********************************/
// if(Angle_flag)
// {
//
// Angle_flag = 0;
// OLED_Shownumf(10, 2, 1, AngleMax_X);
// OLED_Shownumf(10, 4, 1, AngleMin_X);
// AngleMax_X = 0;
// AngleMin_X = 0;
//
//
// }
// OLED_Shownumf(34, 6, 1, Angle_Last.rol);
// max_p_ang_X(&AngleRight, &AngleLift);
// Circle_CC(12.8);
// SimplePendulum_X_CC(18.8);
// SimplePendulum_Y_CC(18.8);
// SimplePendulum_X(); // X轴摆动
// SimplePendulum_Y(); // Y轴摆动
// AnglePendulum(); // 斜角摆动 ...效果不好 风机风力没有校正
// Circle(); // 圆周.. 有可行性
/***************************测试代码***********************************/
}
}
int main()
{
SerialDebug(250000); //PrintString() and PrintFloat() using UART
BeginBasics();
Blink();
Enable_PeriphClock();
/*Roll-0 Pitch-1 Yaw-2..Roll rotation around X axis.Pitch rotation around Y axis and Yaw rotation around Z axis
note: It does not mean rotation along the X Y or Z axis*/
float RPY_c[3],RPY_k[3]; //RPY_c and RPY_k..Roll pitch and yaw obtained from complemntary filter and kalman filter
float Accel[3],Gyro[3],Tempreature; //raw values
float Accel_RealWorld[3]; //Real World Acceleration
while(Init_I2C(400)){PrintString("\nI2C Connection Error");}
while(MPU6050_Init()){PrintString("MPU6050 Initialization Error");}
MPU6050_UpdateOffsets(&MPU6050_Offsets[0]);
//MPU6050_ConfirmOffsets(&MPU6050_Offsets[0]);
while(0) //to play around with quaternions and vector rotation
{
float vector[3]= {1,0,0};
float axis_vector[3]= {0,1,0}; //rotate around Y axis
float rot_angle=90; //with 90 degrees
Quaternion q;
q=RotateVectorY(vector,rot_angle); //Rotates vector around Y axis with rot_angle
PrintString("\nRotated Vector's Quaternion\t");
DisplayQ(q);
PrintString("\n");
}
while(1)
{
MPU6050_GetRaw(&Accel[0],&Gyro[0],&Tempreature); //Reads MPU6050 Raw Data Buffer..i.e Accel Gyro and Tempreature values
if(Gyro[2]<0.3 && Gyro[2]>-0.3) Gyro[2]=0; //this actually reduces Yaw drift..will add magnetometer soon
spudnut=tics(); //tics() return current timing info..using SysTick running at CPU_Core_Frequency/8..Counter Runs from 0xFFFFFF to 0 therefore overflows every 1.864135 secs
delt=spudnut-donut; //small time dt
donut=spudnut;
//Display_Raw(Accel,Gyro,Tempreature);
Attitude_k(Accel,Gyro,RPY_k,delt); //Estimates YPR using Kalman
PrintString("\nYPR\t");
PrintFloat(RPY_k[2]);
PrintString("\t");
PrintFloat(RPY_k[1]);
PrintString("\t");
PrintFloat(RPY_k[0]);
RemoveGravity(RPY_k,Accel,Accel_RealWorld);
PrintString("\tReal World Accel with Gravity\t"); //still glitchish..working on it
DisplayVector(Accel_RealWorld);
PrintString("\t");
PrintFloat(1/delt);
}
}
int main(void)
{
Delay_ms(100);
Periph_clock_enable();
GPIO_Config();
Usart4Init();
I2C_Config();
ADC_Config();
MPU6050_Init();
Timer1_Config();
Timer8_Config();
Timer2_Config();
Timer5_Config();
Timer4_Config();
Timer3_Config();//RC control timer
NVIC_Configuration();
EXTI_Config();
TIM_Cmd(TIM5, ENABLE);
TIM_CtrlPWMOutputs(TIM5, ENABLE);
for (i = 1 ; i < 1 ; i++) ; //small delay before starting Timer4
TIM_Cmd(TIM4, ENABLE);
TIM_CtrlPWMOutputs(TIM4, ENABLE);
Delay_ms(100);
for (i = 0; i < configDataSize; i++) //reads configuration from eeprom
{
ReadFromEEPROM(i);
configData[i] = EepromData;
Delay_ms(5);
}
I2C_AcknowledgeConfig(I2C2, ENABLE);
Delay_ms(100);
while (1)
{
LEDon;
DEBUG_LEDon;
while (ConfigMode == 1)
{
TimerOff(); //Configuration loop
}
MPU6050_ACC_get();//Getting Accelerometer data
acc_roll_angle = -(atan2(accADC_x, accADC_z)) + (configData[11] - 50.00) * 0.0035; //Calculating pitch ACC angle+callibration
acc_pitch_angle = +(atan2(accADC_y, accADC_z)); //Calculating roll ACC angle
MPU6050_Gyro_get();//Getting Gyroscope data
acc_roll_angle_vid = ((acc_roll_angle_vid * 99.00) + acc_roll_angle) / 100.00; //Averaging pitch ACC values
acc_pitch_angle_vid = ((acc_pitch_angle_vid * 99.00) + acc_pitch_angle) / 100.00; //Averaging roll ACC values
sinus = sinusas[(int)(rc4)]; //Calculating sinus
cosinus = sinusas[90 - (int)(rc4)]; //Calculating cosinus
ROLL = -gyroADC_z * sinus + gyroADC_y * cosinus;
roll_angle = (roll_angle + ROLL * dt) + 0.0002 * (acc_roll_angle_vid - roll_angle); //Roll Horizon
//ROLL=-gyroADC_z*sinus+gyroADC_y*cosinus;
yaw_angle = (yaw_angle + gyroADC_z * dt); //Yaw
pitch_angle_true = ((pitch_angle_true + gyroADC_x * dt) + 0.0002 * (acc_pitch_angle_vid - pitch_angle_true)); //Pitch Horizon
ADC1Ch1_vid = ((ADC1Ch1_vid * 99.00) + (readADC1(1) / 4000.00)) / 100.00; //Averaging ADC values
ADC1Ch1_vid = 0.00;
rc4_avg = ((rc4_avg * 499.00) + (rc4)) / 500.00; //Averaging RC4 values
pitch_angle = pitch_angle_true - rc4_avg / 57.3; //Adding angle
pitch_angle_correction = pitch_angle * 150.0;
if (pitch_angle_correction > 2.0)
{
pitch_angle_correction = 2.0;
}
if (pitch_angle_correction < -2.0)
{
pitch_angle_correction = -2.0;
}
pitch_setpoint = pitch_setpoint + pitch_angle_correction; //Pitch return to zero after collision
roll_angle_correction = roll_angle * 200.0;
if (roll_angle_correction > 2.0)
{
roll_angle_correction = 2.0;
}
if (roll_angle_correction < -2.0)
{
roll_angle_correction = -2.0;
}
roll_setpoint = roll_setpoint + roll_angle_correction; //Roll return to zero after collision
ADC1Ch13_vid = ((ADC1Ch13_vid * 99.00) + ((readADC1(13) - 2000) / 4000.00)) / 100.00; //Averaging ADC values
if (configData[10] == '0')
{
yaw_angle = (yaw_angle + gyroADC_z * dt) + 0.01 * (ADC1Ch13_vid - yaw_angle); //Yaw AutoPan
}
if (configData[10] == '1')
{
yaw_angle = (yaw_angle + gyroADC_z * dt); //Yaw RCPan
}
yaw_angle_correction = yaw_angle * 50.0;
if (yaw_angle_correction > 1.0)
{
yaw_angle_correction = 1.0;
}
if (yaw_angle_correction < -1.0)
{
yaw_angle_correction = -1.0;
}
yaw_setpoint = yaw_setpoint + yaw_angle_correction; //Yaw return to zero after collision
pitch_PID();//Pitch axis pid
roll_PID(); //Roll axis pid
yaw_PID(); //Yaw axis pid
printcounter++; //Print data to UART
if (printcounter >= 100)
{
//sprintf (buff, " %d %d %c Labas\n\r", ACCread[0], ACCread[1], ACCread[2]);
//sprintf (buff, " %x %x %x %x %x %x Labas\n\r", ACCread[0], ACCread[1], ACCread[2], ACCread[3], ACCread[4], ACCread[5]);
//sprintf (buff, "Labas %d %d\n\r", ACCread[0], ACCread[1]);
//sprintf (buff, "%3.1f %f\n\r", ADC1Ch1_vid*57.3, sinus);
//sprintf (buff, "Labas %f %f %f \n\r", accADC_x, accADC_y, accADC_z);
//sprintf (buff, "%3.1f %3.1f \n\r", acc_roll_angle_vid*57.3, acc_pitch_angle_vid *57.3);
//sprintf (buff, "%3.1f %3.1f \n\r", pitch_angle*57.3, roll_angle*57.3);
//sprintf (buff, "%d\n\r", rc4);
//USART_PutString(buff);
printcounter = 0;
}
stop = 0;
LEDoff;
watchcounter = 0;
while (stop == 0) {} //Closed loop waits for interrupt
}
}
