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 } }