//-------------- //main loop int main(void) { int i=0; for(i=0;i<100000ul;i++); rcc_config(); nvic_config(); gpio_config(); usart_config(); USART_puts(USART1, "USART BT initialization complete!\r\n"); // just send a message to indicate that it works MPU6050_I2C_Init(); MPU6050_Initialize(); if( MPU6050_TestConnection() == 1){ // connection success USART_puts(USART1, "I2C IMU connection initialization complete!\r\n"); }else{ // connection failed USART_puts(USART1, "I2C initialization failed!\r\n"); } //sysTick_Config_Mod(SysTick_CLKSource_HCLK_Div8, 10500000ul); // interruption every 1/2sec from systick sysTick_Config_Mod(SysTick_CLKSource_HCLK_Div8, 840000ul); // interruption every 0.04sec from systick while(1) { } }
/** Power on and prepare for general usage. * This will activate the device and take it out of sleep mode (which must be done * after start-up). This function also sets both the accelerometer and the gyroscope * to their most sensitive settings, namely +/- 2g and +/- 250 degrees/sec, and sets * the clock source to use the X Gyro for reference, which is slightly better than * the default internal clock source. */ void MPU6050_Init() { DIS_SYSTICK(); MPU6050_I2C_Init(); if( !MPU6050_TestConnection() ) { printf( "init error\r\n" ); while( 1 ) ; } MPU6050_SetClockSource( MPU6050_CLOCK_PLL_XGYRO ); MPU6050_SetFullScaleGyroRange( MPU6050_GYRO_FS_2000 ); MPU6050_SetFullScaleAccelRange( MPU6050_ACCEL_FS_4 ); MPU6050_SetSleepModeStatus( DISABLE ); EN_SYSTICK(); }
int main(){ usart2_init(); delay_init(); MPU6050_I2C_Init(); MPU6050_Initialize(); MPU6050_Exti_Config(); TIM3_Init(72,10000); printf("start\n"); while(1) { } return 0; }
unsigned char mpu_test ( void ) { usart_puts ( 1, "lib MPU6050 - test - enter!\r\n" ); usart_puts ( 1, "lib MPU6050 - test - i2c init!\r\n" ); MPU6050_I2C_Init(); usart_puts ( 1, "lib MPU6050 - test - mpu init!\r\n" ); MPU6050_Initialize(); usart_puts ( 1, "lib MPU6050 - test - test connection!\r\n" ); if ( MPU6050_TestConnection() == FALSE ) { usart_puts ( 1, "Init MPU6050 - FAIL!\r\n" ); return ( 0 ); } // connection success usart_puts ( 1, "Init MPU6050 - success\r\n" ); // run a looping test // int16_t AccelGyro[6]={0}; unsigned char i; char b [ 20 ]; while ( 1 ) { MPU6050_GetRawAccelGyro ( AccelGyro ); usart_puts ( 1, "GyroAccel: " ); for ( i = 0; i <= 5; i++ ) { lame_itoa ( AccelGyro [ i ], b ); usart_puts ( 1, b ); usart_puts ( 1, " , " ); } usart_puts ( 1, "\r\n" ); lame_delay_ms ( 1000 ); } // while return ( 1 ); }
void Initial_MCU(void) { RCC_Configuration(); // Enable Clock GPIO_Configuration(); // Define Indicator LED /*usart*/ DMA1_Channel4_Configuration(); /*i2c*/ DMA1_Channel7_Configuration(); MPU6050_I2C_Init(); MPU6050_Initialize(); USART_Config(USART1, 921600) ; //for display on computer SysTick_cfg(); NVIC_Configuration(); }
int main(void) { int16_t buff[6]; uint8_t bin_buff[13]; comm_package imu_comm; imu_comm.header = (uint8_t)'I'; init_led(); init_usart1(); MPU6050_I2C_Init(); MPU6050_Initialize(); init_tim2(); if( MPU6050_TestConnection() == TRUE) { puts("connection success\r\n"); }else { puts("connection failed\r\n"); } imu_calibration(); while (1) { //puts("running now\r\n"); MPU6050_GetRawAccelGyro(buff); imu_comm.acc_x = buff[0]-ACC_X_OFFSET; imu_comm.acc_y = buff[1]-ACC_Y_OFFSET; imu_comm.acc_z = buff[2]-ACC_Z_OFFSET; imu_comm.gyro_x = buff[3]-GYRO_X_OFFSET; imu_comm.gyro_y = buff[4]-GYRO_Y_OFFSET; imu_comm.gyro_z = buff[5]-GYRO_Z_OFFSET; generate_package( &imu_comm, &bin_buff[0]); for (int i = 0 ; i<13 ; i++) send_byte( bin_buff[i] ); gpio_toggle(GPIOA, GPIO_Pin_0); gpio_toggle(GPIOA, GPIO_Pin_1); delay_ms(10); } }
/** * @brief Main program * @param None * @retval None */ int main(void) { /*!< At this stage the microcontroller clock setting is already configured, this is done through SystemInit() function which is called from startup files (startup_stm32f40xx.s/startup_stm32f427x.s) before to branch to application main. To reconfigure the default setting of SystemInit() function, refer to system_stm32f4xx.c file */ /* USART configuration -----------------------------------------------------*/ USART_Config(); /* SysTick configuration ---------------------------------------------------*/ SysTickConfig(); /* LEDs configuration ------------------------------------------------------*/ STM_EVAL_LEDInit(LED3); STM_EVAL_LEDInit(LED4); STM_EVAL_LEDInit(LED5); STM_EVAL_LEDInit(LED6); STM_EVAL_LEDOn(LED3);//orange STM_EVAL_LEDOn(LED4);//verte STM_EVAL_LEDOn(LED5);//rouge STM_EVAL_LEDOn(LED6);//bleue //PWM config (motor control) TIM1_Config(); PWM1_Config(10000); /* Tamper Button Configuration ---------------------------------------------*/ STM_EVAL_PBInit(BUTTON_USER,BUTTON_MODE_GPIO); //Set motor speed PWM_SetDC(1, SPEED_100); //PE9 | PC6//ON 2ms PWM_SetDC(2, SPEED_100); //PE11 | PC 7 PWM_SetDC(3, SPEED_100); //PE13 PWM_SetDC(4, SPEED_100); //PE14 // /* Wait until Tamper Button is released */ while (STM_EVAL_PBGetState(BUTTON_USER)); PWM_SetDC(1, SPEED_0); //PE9 | PC6//ON 2ms PWM_SetDC(2, SPEED_0); //PE11 | PC 7 PWM_SetDC(3, SPEED_0); //PE13 PWM_SetDC(4, SPEED_0); //PE14 /* Initialization of the accelerometer -------------------------------------*/ MPU6050_I2C_Init(); MPU6050_Initialize(); if (MPU6050_TestConnection()) { // connection success STM_EVAL_LEDOff(LED3); }else{ STM_EVAL_LEDOff(LED4); } //Calibration process // Use the following global variables and access functions // to calibrate the acceleration sensor calibrate_sensors(); zeroError(); //Ready to receive message /* Enable DMA USART RX Stream */ DMA_Cmd(USARTx_RX_DMA_STREAM,ENABLE); /* Enable USART DMA RX Requsts */ USART_DMACmd(USARTx, USART_DMAReq_Rx, ENABLE); while(1){ //-------------------------------------------------------- //------ Used to configure the speed controller ---------- //-------------------------------------------------------- // press blue button to force motor at SPEED_100 if (STM_EVAL_PBGetState(BUTTON_USER)){ PWM_SetDC(1, SPEED_100); //PE9 | PC6//ON 2ms PWM_SetDC(2, SPEED_100); //PE11 | PC 7 PWM_SetDC(3, SPEED_100); //PE13 PWM_SetDC(4, SPEED_100); //PE14 // /* Wait until Tamper Button is released */ while (STM_EVAL_PBGetState(BUTTON_USER)); PWM_SetDC(1, SPEED_0); //PE9 | PC6//ON 2ms PWM_SetDC(2, SPEED_0); //PE11 | PC 7 PWM_SetDC(3, SPEED_0); //PE13 PWM_SetDC(4, SPEED_0); //PE14 Delay(100); } //-------------------------------------------------------- //------ Get gyro information ---------- //-------------------------------------------------------- // Read the raw values. MPU6050_GetRawAccelGyro(AccelGyro); // Get the time of reading for rotation computations unsigned long t_now = millis(); STM_EVAL_LEDToggle(LED5); // The temperature sensor is -40 to +85 degrees Celsius. // It is a signed integer. // According to the datasheet: // 340 per degrees Celsius, -512 at 35 degrees. // At 0 degrees: -512 – (340 * 35) = -12412 //dT = ( (double) AccelGyro[TEMP] + 12412.0) / 340.0; // Convert gyro values to degrees/sec gyro_x = (AccelGyro[GYRO_X] - base_x_gyro) / FSSEL; gyro_y = (AccelGyro[GYRO_Y] - base_y_gyro) / FSSEL; gyro_z = (AccelGyro[GYRO_Z] - base_z_gyro) / FSSEL; // Get raw acceleration values accel_x = AccelGyro[ACC_X]; accel_y = AccelGyro[ACC_Y]; accel_z = AccelGyro[ACC_Z]; // Get angle values from accelerometer //float accel_vector_length = sqrt(pow(accel_x,2) + pow(accel_y,2) + pow(accel_z,2)); float accel_angle_y = atan(-1*accel_x/sqrt(pow(accel_y,2) + pow(accel_z,2)))*RADIANS2DEGREES; float accel_angle_x = atan(accel_y/sqrt(pow(accel_x,2) + pow(accel_z,2)))*RADIANS2DEGREES; //float accel_angle_z = 0; //// Compute the (filtered) gyro angles //Get the value in second, a tick is every 10ms dt = (t_now - last_read_time)/100.0; float gyro_angle_x = gyro_x*dt + lastAngle[X];//get_last_x_angle(); float gyro_angle_y = gyro_y*dt + lastAngle[Y];//(get_last_y_angle(); float gyro_angle_z = gyro_z*dt + lastAngle[Z];//get_last_z_angle(); // Compute the drifting gyro angles float unfiltered_gyro_angle_x = gyro_x*dt + lastGyroAngle[X];//get_last_gyro_x_angle(); float unfiltered_gyro_angle_y = gyro_y*dt + lastGyroAngle[Y];//get_last_gyro_y_angle(); float unfiltered_gyro_angle_z = gyro_z*dt + lastGyroAngle[Z];//get_last_gyro_z_angle(); // Apply the complementary filter to figure out the change in angle – choice of alpha is // estimated now. Alpha depends on the sampling rate… float alpha = 0.96; angle_x = alpha * gyro_angle_x + (1.0 - alpha) * accel_angle_x; angle_y = alpha * gyro_angle_y + (1.0 - alpha) * accel_angle_y; angle_z = gyro_angle_z; //Accelerometer doesn’t give z-angle //printf("%4.2f %4.2f %4.2f\r\n",angle_x,angle_y,angle_z); //// Update the saved data with the latest values set_last_read_angle_data(t_now, angle_x, angle_y, angle_z, unfiltered_gyro_angle_x, unfiltered_gyro_angle_y, unfiltered_gyro_angle_z); //Stabilisation // Stable Mode angl = getAngleFromRC(rcBluetooth[ROLL]); levelRoll = (getAngleFromRC(rcBluetooth[ROLL]) - angle_x) * PID[LEVELROLL].P; levelPitch = (getAngleFromRC(rcBluetooth[PITCH]) - angle_y) * PID[LEVELPITCH].P; // Check if pilot commands are not in hover, don't auto trim // if ((abs(receiver.getTrimData(ROLL)) > levelOff) || (abs(receiver.getTrimData(PITCH)) > levelOff)) { // zeroIntegralError(); // } // else { PID[LEVELROLL].integratedError = constrain(PID[LEVELROLL].integratedError + (((getAngleFromRC(rcBluetooth[ROLL]) - angle_x) * dt) * PID[LEVELROLL].I), -LEVEL_LIMIT, LEVEL_LIMIT); PID[LEVELPITCH].integratedError = constrain(PID[LEVELPITCH].integratedError + (((getAngleFromRC(rcBluetooth[PITCH]) + angle_y) * dt) * PID[LEVELROLL].I), -LEVEL_LIMIT, LEVEL_LIMIT); // } //motors.setMotorAxisCommand(ROLL, motor[ROLL] = updatePID(rcBluetooth[ROLL] + levelRoll, gyro_x + 1500, &PID[LEVELGYROROLL],dt) + PID[LEVELROLL].integratedError;//); //motors.setMotorAxisCommand(PITCH, motor[PITCH] = updatePID(rcBluetooth[PITCH] + levelPitch, gyro_y + 1500, &PID[LEVELGYROPITCH],dt) + PID[LEVELPITCH].integratedError;//); getLastSpeedFromMsg(); PWM_SetDC(1, SPEED_0 + SPEED_RANGE*rcSpeed[1] + motor[ROLL] *0.10f); //PE9 | PC6//ON 2ms //Send data on UART *(float*)(aTxBuffer) = angle_x; *(float*)(aTxBuffer+4) = angle_y; *(float*)(aTxBuffer+8) = angle_z; *(float*)(aTxBuffer+12) = motor[ROLL]; *(float*)(aTxBuffer+16) = motor[PITCH]; sendTxDMA((uint32_t)aTxBuffer,20); //Wait a little bit Delay(3); //30 ms } }
int main(void) { RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOA | RCC_APB2Periph_GPIOB | RCC_APB2Periph_GPIOC | RCC_APB2Periph_GPIOD, ENABLE); motor_init(); uart_init(); printf("Uart started\r\n"); delay_init(); MPU6050_I2C_Init(); MPU6050_Initialize(); printf("MPU started\r\n"); //TIMER CONFIG RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3, ENABLE); TIM_TimeBaseInitTypeDef tim; NVIC_InitTypeDef nvic; TIM_TimeBaseStructInit(&tim); tim.TIM_CounterMode = TIM_CounterMode_Up; tim.TIM_Prescaler = 64000 - 1; tim.TIM_Period = SAMPLERATE - 1; TIM_TimeBaseInit(TIM3, &tim); TIM_ITConfig(TIM3, TIM_IT_Update, ENABLE); TIM_Cmd(TIM3, ENABLE); nvic.NVIC_IRQChannel = TIM3_IRQn; nvic.NVIC_IRQChannelPreemptionPriority = 0; nvic.NVIC_IRQChannelSubPriority = 0; nvic.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&nvic); GPIO_InitTypeDef gpio; GPIO_StructInit(&gpio); gpio.GPIO_Pin = GPIO_Pin_5; gpio.GPIO_Mode = GPIO_Mode_Out_PP; GPIO_Init(GPIOA, &gpio); float AccX=0; float AccY=0; float GyroX=0; float GyroXprev=0; float GyroXbias=0; float GyroXsum=0; float pAcc=1;//something different than 0 float pFil=0; float pWheel=0; float PID1out=0; float PID2out=0; float error1=0; float error1prev=0; float error2=0; float error2prev=0; printf("Calibration stage 1...\r\n"); while(pAcc>0.01||pAcc<-0.01){ MPU6050_GetRawAccelGyro(mpu); AccX=mpu[1]*2.0f/32678.0f; AccY=mpu[2]*2.0f/32678.0f; pAcc=atan(AccX/AccY)*180/3.14; } printf("Calibration stage 2...\r\n"); GPIO_SetBits(GPIOA, GPIO_Pin_5); delay_ms(500); GPIO_ResetBits(GPIOA, GPIO_Pin_5); for(int i=1;i<=10;i++){ MPU6050_GetRawAccelGyro(mpu); GyroXsum+=mpu[3]*250.0f/32678.0f; delay_ms(10); } GPIO_SetBits(GPIOA, GPIO_Pin_5); GyroXbias=GyroXsum/10; printf("%.2f",GyroXbias); MPU6050_GetRawAccelGyro(mpu); pAcc=atan(AccX/AccY)*180/3.14; pFil=0; printf("While started\r\n"); while (1){ while (!flag_freq); flag_freq=0; MPU6050_GetRawAccelGyro(mpu); AccX=mpu[1]*2.0f/32678.0f; AccY=mpu[2]*2.0f/32678.0f; GyroX=mpu[3]*250.0f/32678.0f-GyroXbias; pAcc=(pAcc*9+atan(AccX/AccY)*180/3.14)/10; //in degres pFil=0.96*(pFil+((GyroX+GyroXprev)*SAMPLERATE/2000))+(0.04*pAcc);//complementray filter GyroXprev=GyroX; error1=0-pFil; PID1out=PID(3,0,0.5,error1,error1prev); error2=pWheel; PID2out=PID(0,0,0,error2,error2prev); error1prev=error1; error2prev=error2; //printf("%f\r\n",pAcc); //printf("%f\r\n",pGyro); PID2out=PID1out; //fix here if(PID2out>PIDMAX)PID2out=PIDMAX; else if(PID2out<-PIDMAX)PID2out=-PIDMAX; //printf("%.2f %.2f %.2f \r\n",pFil,pAcc,PID2out); pWheel+=PID2out; if(PID2out>0) forward(PID2out); else backward(-1*PID2out); } }