void init_MiniMU9()
{ 
  I2C_Init();

  delay(1500);

  Accel_Init();
  Compass_Init();
  Gyro_Init();
  
  delay(20);
  
  for(int i=0;i<32;i++)    // We take some readings...
    {
    Read_Gyro();
    Read_Accel();
    for(int y=0; y<6; y++)   // Cumulate values
      AN_OFFSET[y] += AN[y];
    delay(20);
    }
      
  
  for(int y=0; y<6; y++)
    AN_OFFSET[y] = AN_OFFSET[y]/32;
    
  AN_OFFSET[5]-=GRAVITY*SENSOR_SIGN[5];
  
  
  delay(2000);
  
  timer=millis();
  delay(20);
  counter=0;
}
Esempio n. 2
0
void setupImu()
{
	
	// Init sensors
	delay(50);  // Give sensors enough time to start
	I2C_Init();
	Accel_Init();
	Magn_Init();
	Gyro_Init();

	// Read sensors, init DCM algorithm
	delay(20);  // Give sensors enough time to collect data
	reset_sensor_fusion();

}
Esempio n. 3
0
int main()
{
    setvbuf(stdout, NULL, _IONBF, 0);
    setvbuf(stderr, NULL, _IONBF, 0);

    /*!< At this stage the microcontroller clock setting is already configured, 
      this is done through SystemInit() function which is called from startup
      file (startup_stm32f30x.s) before to branch to application main.
      To reconfigure the default setting of SystemInit() function, refer to
      system_stm32f30x.c file
      */ 

    /* SysTick end of count event each 10ms */
    RCC_GetClocksFreq(&RCC_Clocks);
    SysTick_Config(RCC_Clocks.HCLK_Frequency / 100);

    /* initialise USART1 debug output (TX on pin PA9 and RX on pin PA10) */
    USART1_Init();

    //printf("Starting\n");
    USART1_flush();

    /*
    printf("Initialising USB\n");
    USBHID_Init();
    printf("Initialising USB HID\n");
    Joystick_init();
    */
    
    /* Initialise LEDs */
    //printf("Initialising LEDs\n");
    int i;
    for (i = 0; i < 8; ++i) {
        STM_EVAL_LEDInit(leds[i]);
        STM_EVAL_LEDOff(leds[i]);
    }

    /* Initialise gyro */
    //printf("Initialising gyroscope\n");
    Gyro_Init();

    /* Initialise compass */
    //printf("Initialising compass\n");
    Compass_Init();

    Delay(100);
    calibrate();

    int C = 0, noAccelCount = 0;

    while (1) {
        float *acc = accs[C&1],
              *prevAcc = accs[(C&1)^1],
              *vel = vels[C&1],
              *prevVel = vels[(C&1)^1],
              *pos = poss[C&1],
              *prevPos = poss[(C&1)^1],
              *angRate = angRates[C&1],
              *prevAngRate = angRates[(C&1)^1],
              *ang = angs[C&1],
              *prevAng = angs[(C&1)^1],
              *mag = mags[C&1],
              *prevmag = mags[(C&1)^1];

        /* Wait for data ready */

#if 0
        Compass_ReadAccAvg(acc, 10);
        vecMul(axes, acc);
        //printf("X: %9.3f Y: %9.3f Z: %9.3f\n", acc[0], acc[1], acc[2]);
        
        float grav = acc[2];
        acc[2] = 0;
        
        if (noAccelCount++ > 50) {
            for (i = 0; i < 2; ++i) {
                vel[i] = 0;
                prevVel[i] = 0;
            }
            noAccelCount = 0;
        }

        if (vecLen(acc) > 50.f) {
            for (i = 0; i < 2; ++i) {
                vel[i] += prevAcc[i] + (acc[i]-prevAcc[i])/2.f;
                pos[i] += prevVel[i] + (vel[i]-prevVel[i])/2.f;
            }
            noAccelCount = 0;
        }

        C += 1;
        if (((C) & 0x7F) == 0) {
            printf("%9.3f %9.3f %9.3f %9.3f %9.3f\n", vel[0], vel[1], pos[0], pos[1], grav);
            //printf("%3.1f%% %d %5.1f %6.3f\n", (float) timeReadI2C*100.f / totalTime, C, (float) C*100.f / (totalTime), grav);
        }
#endif

        Compass_ReadMagAvg(mag, 2);
        vecMul(axes, mag);
        float compassAngle = atan2f(mag[1], mag[0]) * 180.f / PI;
        if (compassAngle > 180.f) compassAngle -= 360.f;
        //vecNorm(mag);
        

        Gyro_ReadAngRateAvg(mag, 2);
        printf("%6.3f:%6.3f,%6.3f,%6.3f\n", compassAngle, mag[0], mag[1], mag[2]);

#if 0
        Gyro_ReadAngRateAvg(angRate, 2);
        angRate[0] *= 180.f / PI;
        angRate[1] *= 180.f / PI;
        angRate[2] *= 180.f / PI;
        float s[3] = {sin(angRate[0]), sin(angRate[1]), sin(angRate[2])};
        float c[3] = {cos(angRate[0]), cos(angRate[1]), cos(angRate[2])};
        float gyroMat[3][3] = {
            {c[0]*c[1], c[0]*s[1], -s[1]},
            {c[0]*s[1]*s[2]-s[0]*c[2], c[0]*c[2]+s[0]*s[1]*s[2], c[1]*s[2]},
            {c[0]*s[1]*c[2]+s[0]*s[2], -c[0]*s[2]+s[0]*s[1]*c[2], c[1]*c[2]}};
        /*
        float gyroWorldMat[3][3];
        vecMulMatTrans(gyroWorldMat, axes, gyroMat);
        *ang = gyroWorldMat[2][0];
        *ang += gyroWorldMat[2][1];
        *ang += gyroWorldMat[2][2];
        *ang /= 300.f;
        static const float ANGALPHA = 0.0f;
        *ang += ANGALPHA*(compassAngle - *ang);
        */
        float rotObsVec[3];
        memcpy(rotObsVec, axes[0], sizeof(rotObsVec));
        vecMul(gyroMat, rotObsVec);
        vecMul(axes, rotObsVec);
        rotObsVec[2] = 0.f;
        vecNorm(rotObsVec);
        float angDelta = acos(rotObsVec[0]);

        if (((++C) & 0x7) == 0) {
            printf("%6.3f %6.3f %6.3f %6.3f\n", rotObsVec[0], rotObsVec[1], rotObsVec[2], angDelta);
        }
#endif


#if 0
        float angRate[3];
      
      /* Read Gyro Angular data */
      Gyro_ReadAngRate(angRate);

      printf("X: %f Y: %f Z: %f\n", angRate[0], angRate[1], angRate[2]);

        float MagBuffer[3] = {0.0f}, AccBuffer[3] = {0.0f};
        float fNormAcc,fSinRoll,fCosRoll,fSinPitch,fCosPitch = 0.0f, RollAng = 0.0f, PitchAng = 0.0f;
        float fTiltedX,fTiltedY = 0.0f;


      Compass_ReadMag(MagBuffer);
      Compass_ReadAcc(AccBuffer);
      
      for(i=0;i<3;i++)
        AccBuffer[i] /= 100.0f;
      
      fNormAcc = sqrt((AccBuffer[0]*AccBuffer[0])+(AccBuffer[1]*AccBuffer[1])+(AccBuffer[2]*AccBuffer[2]));
      
      fSinRoll = -AccBuffer[1]/fNormAcc;
      fCosRoll = sqrt(1.0-(fSinRoll * fSinRoll));
      fSinPitch = AccBuffer[0]/fNormAcc;
      fCosPitch = sqrt(1.0-(fSinPitch * fSinPitch));
     if ( fSinRoll >0)
     {
       if (fCosRoll>0)
       {
         RollAng = acos(fCosRoll)*180/PI;
       }
       else
       {
         RollAng = acos(fCosRoll)*180/PI + 180;
       }
     }
     else
     {
       if (fCosRoll>0)
       {
         RollAng = acos(fCosRoll)*180/PI + 360;
       }
       else
       {
         RollAng = acos(fCosRoll)*180/PI + 180;
       }
     }
     
      if ( fSinPitch >0)
     {
       if (fCosPitch>0)
       {
            PitchAng = acos(fCosPitch)*180/PI;
       }
       else
       {
          PitchAng = acos(fCosPitch)*180/PI + 180;
       }
     }
     else
     {
       if (fCosPitch>0)
       {
            PitchAng = acos(fCosPitch)*180/PI + 360;
       }
       else
       {
          PitchAng = acos(fCosPitch)*180/PI + 180;
       }
     }

      if (RollAng >=360)
      {
        RollAng = RollAng - 360;
      }
      
      if (PitchAng >=360)
      {
        PitchAng = PitchAng - 360;
      }
      
      fTiltedX = MagBuffer[0]*fCosPitch+MagBuffer[2]*fSinPitch;
      fTiltedY = MagBuffer[0]*fSinRoll*fSinPitch+MagBuffer[1]*fCosRoll-MagBuffer[1]*fSinRoll*fCosPitch;
      
      HeadingValue = (float) ((atan2f((float)fTiltedY,(float)fTiltedX))*180)/PI;
 
        printf("Compass heading: %f\n", HeadingValue);
#endif
    }

    return 1;
}
Esempio n. 4
0
int main()
{
    setvbuf(stdout, NULL, _IONBF, 0);
    setvbuf(stderr, NULL, _IONBF, 0);

    /*!< At this stage the microcontroller clock setting is already configured,
      this is done through SystemInit() function which is called from startup
      file (startup_stm32f30x.s) before to branch to application main.
      To reconfigure the default setting of SystemInit() function, refer to
      system_stm32f30x.c file
      */

    /* SysTick end of count event each 10ms */
    RCC_GetClocksFreq(&RCC_Clocks);
    SysTick_Config(RCC_Clocks.HCLK_Frequency / 100);

    /* initialise USART1 debug output (TX on pin PA9 and RX on pin PA10) */
    USART1_Init();

    //printf("Starting\n");
    USART1_flush();

    /* Initialise LEDs */
    //printf("Initialising LEDs\n");
    int i;
    for (i = 0; i < 8; ++i) {
        STM_EVAL_LEDInit(leds[i]);
        STM_EVAL_LEDOff(leds[i]);
    }

    /* Initialise gyro */
    //printf("Initialising gyroscope\n");
    Gyro_Init();

    /* Initialise compass */
    //printf("Initialising compass\n");
    Compass_Init();

    Delay(100);
    // perform calibration
    calibrate();


    while (1) {
        float angRate[3], mag[3];

        // read average compass values
        Compass_ReadMagAvg(mag, 2);
        // rotate the compass values so that they are aligned with Earth
        vecMul(axes, mag);
        // calculate the heading through inverse tan of the Y/X magnetic strength
        float compassAngle = atan2f(mag[1], mag[0]) * 180.f / PI;
        // fix heading to be in range -180 to 180
        if (compassAngle > 180.f) compassAngle -= 360.f;
        // read average gyro values
        Gyro_ReadAngRateAvg(angRate, 2);
        // print out everything
        printf("c%6.3f\ng%6.3f\n", compassAngle, angRate[2]-zeroAngRate[2]);

    }

    return 1;
}
Esempio n. 5
0
void run_function2(void){
	uint8_t Xval, Yval = 0x00;
	int16_t IMU_Buffer[3];
	unsigned char buff[100];
	float IMU_Buffer1[3];
	uint8_t speed=0, accuracy=0; 
	int sum = 0;
	uint32_t ADC_temp = 0;
	if(VCP_receive_string(buff)){//if receive command
		if(buff[0]=='C'){
			Button_state = BUTTON_STATE_1;//stop sending message
			switch(buff[1]){
				case 'A': 
								Read_Acc(IMU_Buffer);
									switch(buff[2]){
											case 'X': VCP_send_IMU_Single(ACC_INDEX,IMU_Buffer[0]);break;
											case 'Y': VCP_send_IMU_Single(ACC_INDEX,IMU_Buffer[1]);break; 
											case 'Z': VCP_send_IMU_Single(ACC_INDEX,IMU_Buffer[2]);break; 
											case 'A': VCP_send_IMU_Multi(ACC_INDEX,IMU_Buffer[0],IMU_Buffer[1],IMU_Buffer[2]);break;
											default :break;
									} 
									break; 
				case 'G': //GyroReadAngRate(IMU_Buffer);
									Read_Gyro(IMU_Buffer);
									switch(buff[2]){
											case 'X': VCP_send_IMU_Single(GYRO_INDEX,IMU_Buffer[0]);break;
											case 'Y': VCP_send_IMU_Single(GYRO_INDEX,IMU_Buffer[1]);break; 
											case 'Z': VCP_send_IMU_Single(GYRO_INDEX,IMU_Buffer[2]);break; 
											case 'A': VCP_send_IMU_Multi(GYRO_INDEX,IMU_Buffer[0],IMU_Buffer[1],IMU_Buffer[2]);break;
											default :break;
									} 
									break;
				case 'C': //LSM303DLHC_CompassReadMag(IMU_Buffer);
									Read_Compass(IMU_Buffer);
									switch(buff[2]){
											case 'X': VCP_send_IMU_Single(COMPASS_INDEX,IMU_Buffer[0]);break;
											case 'Y': VCP_send_IMU_Single(COMPASS_INDEX,IMU_Buffer[1]);break; 
											case 'Z': VCP_send_IMU_Single(COMPASS_INDEX,IMU_Buffer[2]);break; 
											case 'A': VCP_send_IMU_Multi(COMPASS_INDEX,IMU_Buffer[0],IMU_Buffer[1],IMU_Buffer[2]);break;
											default :break;
									} 
									break;
				case 'I': speed = buff[3]-'0';accuracy = buff[4]-'0';
									switch(buff[2]){
											case 'A': Acc_Init(speed,accuracy);break;
											case 'C': Compass_Init(speed,accuracy);break;
											case 'G': Gyro_Init(speed,accuracy);break;
											default : break;
									} 
									break;
				case 'M':sum =(buff[5]-'0')*100 + (buff[6]-'0')*10+(buff[7]-'0');
									if(buff[2] == 'P'){//percentage
										if(sum>100)
											sum =100;
										if(sum<0)
											sum = 0;
										//convert to steps
										if(buff[4]=='-') 
											sum = 100 - sum;  
										else if(buff[4]=='+')
											sum = 100 + sum;
										else 
											break;
										sum = sum * 255 / 200;
										//set the speed
										if(buff[3] == 'L')
											Potentialmeter_SetValue(sum,CHIP1); 
										else if(buff[3] == 'R')
											Potentialmeter_SetValue(sum,CHIP2); 
									
									}else if(buff[2]=='S'){//steps
										if(sum>255)
											sum =0;
										if(sum<0)
											sum = 0;
										//buffer4 is no use
										//set the speed
										if(buff[3] == 'L')
											Potentialmeter_SetValue(sum,CHIP1); 
										else if(buff[3] == 'R')
											Potentialmeter_SetValue(sum,CHIP2); 
									}else 
										break;
									break;
				case 'L':if(buff[3]=='1') STM_EVAL_LEDOn(buff[2]-'1'); else STM_EVAL_LEDOff(buff[2]-'1'); break;
				case 'S': sum = (buff[2]-'0')*100+(buff[3]-'0')*10+(buff[4]-'0') ;Sample_time=sum; break;
				default :break;				
			}	
				Button_state = BUTTON_STATE_3;
		}
	}
	State2_count ++;
	if(State2_count > 200000){
		State2_count =0;
		STM_EVAL_LEDToggle(LED4);
	}
	
	/*
	ADC_temp = ADC3ConvertedValue*3000/0xFFF;
	if(ADC_temp > 0&&ADC_temp<1000)
		STM_EVAL_LEDOn(LED3);
	else if(ADC_temp > 1000&&ADC_temp<2000 )
		STM_EVAL_LEDOn(LED4);
	else if(ADC_temp > 2000&&ADC_temp<3000 )
		STM_EVAL_LEDOn(LED5);
	else{
		STM_EVAL_LEDOff(LED3);
		STM_EVAL_LEDOff(LED4);
		STM_EVAL_LEDOff(LED5);
	}
	VCP_send_int((uint16_t)ADC_temp);
	wait(1000);*/
}
Esempio n. 6
0
void run_function1(void){
//DATA TRANSMISSION
	unsigned char buff[100];
	float IMU_Buffer1[3];
	  //float Buffer[6];
  uint8_t Xval, Yval = 0x00;
	int16_t IMU_Buffer[3];
//	unsigned char start[] = "start";
//	LSM303DLHC_MEMS_Test();
	int speed=0, accuracy=0; 
		int sum = 0;
	if(VCP_receive_string(buff)){//if receive command
		if(buff[0]=='C'){
			switch(buff[1]){
				case 'A': 
								Read_Acc(IMU_Buffer);
									switch(buff[2]){
											case 'X': VCP_send_IMU_Single(ACC_INDEX,IMU_Buffer[0]);break;
											case 'Y': VCP_send_IMU_Single(ACC_INDEX,IMU_Buffer[1]);break; 
											case 'Z': VCP_send_IMU_Single(ACC_INDEX,IMU_Buffer[2]);break; 
											case 'A': VCP_send_IMU_Multi(ACC_INDEX,IMU_Buffer[0],IMU_Buffer[1],IMU_Buffer[2]);break;
											default :break;
									} 
									break; 
				case 'G': //GyroReadAngRate(IMU_Buffer);
									Read_Gyro(IMU_Buffer);
									switch(buff[2]){
											case 'X': VCP_send_IMU_Single(GYRO_INDEX,IMU_Buffer[0]);break;
											case 'Y': VCP_send_IMU_Single(GYRO_INDEX,IMU_Buffer[1]);break; 
											case 'Z': VCP_send_IMU_Single(GYRO_INDEX,IMU_Buffer[2]);break; 
											case 'A': VCP_send_IMU_Multi(GYRO_INDEX,IMU_Buffer[0],IMU_Buffer[1],IMU_Buffer[2]);break;
											default :break;
									} 
									break;
				case 'C': //LSM303DLHC_CompassReadMag(IMU_Buffer);
									Read_Compass(IMU_Buffer);
									switch(buff[2]){
											case 'X': VCP_send_IMU_Single(COMPASS_INDEX,IMU_Buffer[0]);break;
											case 'Y': VCP_send_IMU_Single(COMPASS_INDEX,IMU_Buffer[1]);break; 
											case 'Z': VCP_send_IMU_Single(COMPASS_INDEX,IMU_Buffer[2]);break; 
											case 'A': VCP_send_IMU_Multi(COMPASS_INDEX,IMU_Buffer[0],IMU_Buffer[1],IMU_Buffer[2]);break;
											default :break;
									} 
									break;
				case 'I': speed = buff[3]-'0';accuracy = buff[4]-'0';
									switch(buff[2]){
											case 'A': Acc_Init(speed,accuracy);break;
											case 'C': Compass_Init(speed,accuracy);break;
											case 'G': Gyro_Init(speed,accuracy);break;
											default : break;
									} 
									break;
				case 'M':sum =(buff[5]-'0')*100 + (buff[6]-'0')*10+(buff[7]-'0');
									if(buff[2] == 'P'){//percentage
										if(sum>100)
											sum =100;
										if(sum<0)
											sum = 0;
										//convert to steps
										if(buff[4]=='-') 
											sum = 100 - sum;  
										else if(buff[4]=='+')
											sum = 100 + sum;
										else 
											break;
										sum = sum * 255 / 200;
										//set the speed
										if(buff[3] == 'L')
											Potentialmeter_SetValue(sum,CHIP1); 
										else if(buff[3] == 'R')
											Potentialmeter_SetValue(sum,CHIP2); 
									
									}else if(buff[2]=='S'){//steps
										if(sum>255)
											sum =0;
										if(sum<0)
											sum = 0;
										//buffer4 is no use
										//set the speed
										if(buff[3] == 'L')
											Potentialmeter_SetValue(sum,CHIP1); 
										else if(buff[3] == 'R')
											Potentialmeter_SetValue(sum,CHIP2); 
									}else 
										break;
									break;
				case 'L':if(buff[3]=='1') STM_EVAL_LEDOn(buff[2]-'1'); else STM_EVAL_LEDOff(buff[2]-'1'); break;
				case 'S': sum = (buff[2]-'0')*100+(buff[3]-'0')*10+(buff[4]-'0') ;Sample_time=sum; break;
				default :break;				
			}	
		}

	}
		
//	Potentialmeter_SetValue(i++,CHIP1);
	
//	VCP_receive_string(buff);
//	VCP_send_str(buff);
	/*
//MEMS303	
	LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_X_H_M, 6, buff);
	VCP_send_str(buff);
	VCP_put_char(100);
	LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_X_L_M, 6, buff);
	VCP_send_str(buff);
	VCP_put_char(100);
	LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Y_H_M, 6, buff);
	VCP_send_str(buff);
	VCP_put_char(100);
	LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Y_L_M, 6, buff);
	VCP_send_str(buff);
	VCP_put_char(100);
	LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Z_H_M, 6, buff);
	VCP_send_str(buff);
	VCP_put_char(100);
	LSM303DLHC_Read(MAG_I2C_ADDRESS, LSM303DLHC_OUT_Z_L_M, 6, buff);
	VCP_send_str(buff);
	VCP_put_char(101);
	//VCP_put_char('\n');
	
	L3GD20_Read(buff,L3GD20_OUT_X_L_ADDR,6);
	VCP_send_str(buff);
	VCP_put_char(102);
	L3GD20_Read(buff,L3GD20_OUT_X_H_ADDR,6);
	VCP_send_str(buff);
	VCP_put_char(102);
	L3GD20_Read(buff,L3GD20_OUT_Y_L_ADDR,6);
	VCP_send_str(buff);
	VCP_put_char(102);
	L3GD20_Read(buff,L3GD20_OUT_Y_H_ADDR,6);
	VCP_send_str(buff);
	VCP_put_char(102);
	L3GD20_Read(buff,L3GD20_OUT_Z_L_ADDR,6);
	VCP_send_str(buff);
	VCP_put_char(102);
	L3GD20_Read(buff,L3GD20_OUT_Z_H_ADDR,6);
	VCP_send_str(buff);
	VCP_put_char(103);
	
	
	*/
	
  
  /* Demo Gyroscope */
  //Demo_GyroConfig();
  /*
  // Read Gyro Angular data 
  Read_Gyro(IMU_Buffer);
  
  // Update autoreload and capture compare registers value
  Xval = ABS(IMU_Buffer[0]);
  Yval = ABS(IMU_Buffer[1]);
  
  if ( Xval>Yval)
  {
    if (IMU_Buffer[0] > 1115.0f)
    {       
      STM_EVAL_LEDOn(LED4);
      STM_EVAL_LEDOff(LED3);
      STM_EVAL_LEDOff(LED5);
      STM_EVAL_LEDOff(LED6);
    }
    if (IMU_Buffer[0] < -1115.0f)
    {
      STM_EVAL_LEDOn(LED5);
      STM_EVAL_LEDOff(LED3);
      STM_EVAL_LEDOff(LED4);
      STM_EVAL_LEDOff(LED6);
    }
  }
  else
  {
    if (IMU_Buffer[1]< -1115.0f)
    {
      STM_EVAL_LEDOn(LED3);
      STM_EVAL_LEDOff(LED4);
      STM_EVAL_LEDOff(LED5);
      STM_EVAL_LEDOff(LED6);
    }
    if (IMU_Buffer[1] > 1115.0f)
    {
      STM_EVAL_LEDOn(LED6);
      STM_EVAL_LEDOff(LED3);
      STM_EVAL_LEDOff(LED4);
      STM_EVAL_LEDOff(LED5);
    } 
  } 
	
	
	LSM303DLHC_CompassReadAcc(IMU_Buffer1);
	Xval = ABS(IMU_Buffer1[0]);
  Yval = ABS(IMU_Buffer1[1]);
  
  if ( Xval>Yval)
  {
    if (IMU_Buffer1[0] > 1115.0f)
    {       
      STM_EVAL_LEDOn(LED4);
      STM_EVAL_LEDOff(LED3);
      STM_EVAL_LEDOff(LED5);
      STM_EVAL_LEDOff(LED6);
    }
    if (IMU_Buffer1[0] < -1115.0f)
    {
      STM_EVAL_LEDOn(LED5);
      STM_EVAL_LEDOff(LED3);
      STM_EVAL_LEDOff(LED4);
      STM_EVAL_LEDOff(LED6);
    }
  }
  else
  {
    if (IMU_Buffer1[1]< -1115.0f)
    {
      STM_EVAL_LEDOn(LED3);
      STM_EVAL_LEDOff(LED4);
      STM_EVAL_LEDOff(LED5);
      STM_EVAL_LEDOff(LED6);
    }
    if (IMU_Buffer1[1] > 1115.0f)
    {
      STM_EVAL_LEDOn(LED6);
      STM_EVAL_LEDOff(LED3);
      STM_EVAL_LEDOff(LED4);
      STM_EVAL_LEDOff(LED5);
    } 
  } 
	//MEMS303	
	LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_OUT_X_L_A, 6, buff);
	VCP_put_char(buff[0]);
	LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_OUT_X_H_A, 6, buff);
	VCP_put_char(buff[0]);
	LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_OUT_Y_L_A, 6, buff);
	VCP_put_char(buff[0]);
	LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_OUT_Y_H_A, 6, buff);
	VCP_put_char(buff[0]);
	LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_OUT_Z_L_A, 6, buff);
	VCP_put_char(buff[0]);
	LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_OUT_Z_H_A, 6, buff);
	VCP_put_char(buff[0]);
	
	LSM303DLHC_Read(ACC_I2C_ADDRESS, LSM303DLHC_OUT_X_L_A, 6, buff);
	VCP_send_str(buff);
	//VCP_put_char('\n');*/
	STM_EVAL_LEDOn(LED5);
	wait(10000);
//	Potentialmeter_SetValue(i++,CHIP2);
	STM_EVAL_LEDOff(LED5);
	wait(10000);

	
}