kkmonster

/**
  ******************************************************************************
  * File Name          : main.c
  * Date               : 03/11/2014 02:03:07
  * Description        : Main program body
  ******************************************************************************
  *
  * COPYRIGHT(c) 2014 STMicroelectronics
  *
  * Redistribution and use in source and binary forms, with or without modification,

kkmonster

/**
  ******************************************************************************
  * @file    stm32f0xx_hal_uart.c
  * @author  MCD Application Team
  * @version V1.0.1
  * @date    18-June-2014
  * @brief   UART HAL module driver.
  *    
  *          This file provides firmware functions to manage the following 
  *          functionalities of the UART peripheral:

kkmonster

ร่วมส่งหุ่นยนต์เข้าแข่งขันในงาน Maker Party : Sumo-Bots Battle

วันที่ 28 มีนาคม 2558 รางวัลสำหรับผู้ชนะ 10,000 บาท

สมัครเข้าร่วมแข่งขัน

วันแข่งขัน

วันที่ : วันเสาร์ 28 มีนาคม 2558
สถานที่จัดงาน : ชมรม Chiang Mai Maker Club

kkmonster

ร่วมส่งหุ่นยนต์เข้าแข่งขันในงาน Maker Party : Robots Survival Race

วันที่ 29 มีนาคม 2558 ชิงรางวัลมูลค่ากว่า 5,000 บาท

สมัครเข้าร่วมแข่งขัน

วันแข่งขัน

วันที่ : วันอาทิตย์ 29 มีนาคม 2558
สถานที่จัดงาน : ชมรม Chiang Mai Maker Club

kkmonster

ร่วมส่งหุ่นยนต์เข้าแข่งขันในงาน Maker Party : Robots Survival Race

วันที่ 29 มีนาคม 2558 ชิงรางวัลมูลค่ากว่า 5,000 บาท

สมัครเข้าร่วมแข่งขัน

วันแข่งขัน

วันที่ : วันอาทิตย์ 29 มีนาคม 2558
สถานที่จัดงาน : ชมรม Chiang Mai Maker Club

kkmonster

/* Copyright (c) 2014 Nordic Semiconductor. All Rights Reserved.
 *
 * The information contained herein is property of Nordic Semiconductor ASA.
 * Terms and conditions of usage are described in detail in NORDIC
 * SEMICONDUCTOR STANDARD SOFTWARE LICENSE AGREEMENT.
 *
 * Licensees are granted free, non-transferable use of the information. NO
 * WARRANTY of ANY KIND is provided. This heading must NOT be removed from
 * the file.
 *

kkmonster

void Initial_MPU6050(void)
	{

				//    Reset to defalt 
			MPU6050_WriteBit(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, ENABLE);
				//	  SetClockSource(MPU6050_CLOCK_PLL_XGYRO)
			MPU6050_WriteBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_XGYRO);	
				//    SetFullScaleGyroRange(MPU6050_GYRO_FS_250)
			MPU6050_WriteBits(MPU6050_DEFAULT_ADDRESS, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS_250);
				//    SetFullScaleAccelRange(MPU6050_ACCEL_FS_2)

kkmonster

//MPU6050 I2C library for ARM STM32F103xx Microcontrollers - Main header file 
//Has bit, byte and buffer I2C R/W functions
// 23/05/2012 by Harinadha Reddy Chintalapalli <harinath.ec@gmail.com>
// Changelog:
//     2012-05-23 - initial release. Thanks to Jeff Rowberg <jeff@rowberg.net> for his AVR/Arduino
//                  based development which inspired me & taken as reference to develop this.
/* ============================================================================================
 MPU6050 device I2C library code for ARM STM32F103xx is placed under the MIT license
 Copyright (c) 2012 Harinadha Reddy Chintalapalli

kkmonster

#define beta                        0.1f     
#define ACCELEROMETER_SENSITIVITY   16384.0f  
#define GYROSCOPE_SENSITIVITY       131.07f  
#define M_PI                        3.14159265359f	    
#define sampleFreq                  250.0f     			    // 250 hz sample rate!   



void Initial_MPU6050(void);
void read_mpu6050(void);

kkmonster

void ahrs(void)
{
	// quaternion base process 
	float Norm;
	float ax = AccelGyro[0];
	float ay = AccelGyro[1];
	float az = AccelGyro[2];
	float gx =((AccelGyro[3]-gx_diff)/ GYROSCOPE_SENSITIVITY )*M_PI/180 ;
	float gy =((AccelGyro[4]-gy_diff)/ GYROSCOPE_SENSITIVITY )*M_PI/180 ;
	float gz =((AccelGyro[5]-gz_diff)/ GYROSCOPE_SENSITIVITY )*M_PI/180 ;