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;
}
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();
}
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;
}
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;
}
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);*/
}
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);
}