Gesture Drive: Accelerate with Freedom

/*
 * Copyright (c) 2015, Freescale Semiconductor, Inc.
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without modification,
 * are permitted provided that the following conditions are met:
 *
 * o Redistributions of source code must retain the above copyright notice, this list
 *   of conditions and the following disclaimer.
 *
 * o Redistributions in binary form must reproduce the above copyright notice, this
 *   list of conditions and the following disclaimer in the documentation and/or
 *   other materials provided with the distribution.
 *
 * o Neither the name of Freescale Semiconductor, Inc. nor the names of its
 *   contributors may be used to endorse or promote products derived from this
 *   software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR
 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
 * ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 */

#include "fsl_debug_console.h"
#include "board.h"
#include "math.h"
#include "fsl_fxos.h"
#include "fsl_i2c.h"
#include "fsl_ftm.h"
#include "fsl_lpuart.h"
#include "fsl_common.h"
#include "pin_mux.h"
/*******************************************************************************
 * Definitions
 ******************************************************************************/
#define BOARD_TIMER_BASEADDR TPM3

//============= Change Made here for 3rd channel ==================//
#define BOARD_FIRST_TIMER_CHANNEL 6U
#define BOARD_SECOND_TIMER_CHANNEL 5U
#define BOARD_THIRD_TIMER_CHANNEL 4U


/* Get source clock for TPM driver */
#define BOARD_TIMER_SOURCE_CLOCK CLOCK_GetFreq(BUS_CLK)
/* I2C source clock */
#define ACCEL_I2C_CLK_SRC I2C3_CLK_SRC
#define I2C_BAUDRATE 100000U
/* Upper bound and lower bound angle values */

// UART for BT
#define BT_LPUART LPUART1
#define LPUART_CLKSRC kCLOCK_Osc0ErClk


//======================= Change Made here for Gearing Range ===========//
int ANGLE_UPPER_BOUND = 90U;
int ANGLE_LOWER_BOUND = 10U;

int16_t xData = 0;
 int16_t yData = 0;
 int16_t xAngle = 0;
 int16_t xAngle_Gear=0;
 int16_t yAngle_Left = 0;
 int16_t yAngle_Right = 0;
// Make Function to Update Power level from gear counter and accele counter and angle
 uint8_t power_level=160;

 uint8_t car_cmd[5];

 fxos_handle_t fxosHandle = {0};
    fxos_data_t sensorData = {0};

    uint8_t  Ardu_Left_Wheel_Pwr_Min=1;
    uint8_t  Ardu_Right_Wheel_Pwr_Min=1;


    uint8_t  Ardu_Left_Wheel_Pwr_Max=240;
    uint8_t  Ardu_Right_Wheel_Pwr_Max=240;
    uint8_t Accel_Level=1; // max 8

/*******************************************************************************
 * Prototypes
 ******************************************************************************/


/*******************************************************************************
 * Variables
 ******************************************************************************/
i2c_master_handle_t g_MasterHandle;
/* FXOS device address */
const uint8_t g_accel_address[] = {0x1CU, 0x1DU, 0x1EU, 0x1FU};
uint8_t gear_counter=1U;

/*******************************************************************************
 * Code
 ******************************************************************************/
/* Initialize timer module */
static void Timer_Init(void)
{
    ftm_config_t ftmInfo;

    //============================== Change made here for 3 PWM ==========================//
    //ftm_chnl_pwm_signal_param_t ftmParam[2];
    ftm_chnl_pwm_signal_param_t ftmParam[3];
    /* Configure ftm params with frequency 24kHZ */
    ftmParam[0].chnlNumber = (ftm_chnl_t)BOARD_FIRST_TIMER_CHANNEL;
    ftmParam[0].level = kFTM_LowTrue;
    ftmParam[0].dutyCyclePercent = 0U;
    ftmParam[0].firstEdgeDelayPercent = 0U;

    ftmParam[1].chnlNumber = (ftm_chnl_t)BOARD_SECOND_TIMER_CHANNEL;
    ftmParam[1].level = kFTM_LowTrue;
    ftmParam[1].dutyCyclePercent = 0U;
    ftmParam[1].firstEdgeDelayPercent = 0U;

// ================== 3rf PWM ========================================
    ftmParam[2].chnlNumber = (ftm_chnl_t)BOARD_THIRD_TIMER_CHANNEL;
    ftmParam[2].level = kFTM_LowTrue;
    ftmParam[2].dutyCyclePercent = 0U;
    ftmParam[2].firstEdgeDelayPercent = 0U;

    /*
     * ftmInfo.prescale = kFTM_Prescale_Divide_1;
     * ftmInfo.bdmMode = kFTM_BdmMode_0;
     * ftmInfo.pwmSyncMode = kFTM_SoftwareTrigger;
     * ftmInfo.reloadPoints = 0;
     * ftmInfo.faultMode = kFTM_Fault_Disable;
     * ftmInfo.faultFilterValue = 0;
     * ftmInfo.deadTimePrescale = kFTM_Deadtime_Prescale_1;
     * ftmInfo.deadTimeValue = 0;
     * ftmInfo.extTriggers = 0;
     * ftmInfo.chnlInitState = 0;
     * ftmInfo.chnlPolarity = 0;
     * ftmInfo.useGlobalTimeBase = false;
     */
    FTM_GetDefaultConfig(&ftmInfo);
    /* Initialize FTM module */
    FTM_Init(BOARD_FTM_BASEADDR, &ftmInfo);

    // CHanged Here from 2U to 3U for 3 channel support //
    FTM_SetupPwm(BOARD_FTM_BASEADDR, ftmParam, 3U, kFTM_EdgeAlignedPwm, 2000U, BOARD_TIMER_SOURCE_CLOCK);
    FTM_StartTimer(BOARD_FTM_BASEADDR, kFTM_SystemClock);
}

/* Update the duty cycle of an active pwm signal */
// =================Changed HERE for 3 PWM ======================//
static void Board_UpdatePwm(uint16_t x, uint16_t y,uint16_t a)
{
    /* Start PWM mode with updated duty cycle */
    FTM_UpdatePwmDutycycle(BOARD_FTM_BASEADDR, (ftm_chnl_t)BOARD_FIRST_TIMER_CHANNEL, kFTM_EdgeAlignedPwm, x);
    FTM_UpdatePwmDutycycle(BOARD_FTM_BASEADDR, (ftm_chnl_t)BOARD_SECOND_TIMER_CHANNEL, kFTM_EdgeAlignedPwm, y);

    FTM_UpdatePwmDutycycle(BOARD_FTM_BASEADDR, (ftm_chnl_t)BOARD_THIRD_TIMER_CHANNEL, kFTM_EdgeAlignedPwm, a);

    /* Software trigger to update registers */
    FTM_SetSoftwareTrigger(BOARD_FTM_BASEADDR, true);
}

/*===============================
 *
 * CHANGE MAFE HERE
 *
=============================================================//
 */

void DIAL_OFF(void)
{
				GPIO_WritePinOutput (GPIOC, 3U, 0);
                GPIO_WritePinOutput (GPIOC, 5U, 0);
                GPIO_WritePinOutput (GPIOD, 1U, 0);
                GPIO_WritePinOutput (GPIOD, 2U, 0);
                GPIO_WritePinOutput (GPIOD, 3U, 0);
                GPIO_WritePinOutput (GPIOD, 4U, 0);
                GPIO_WritePinOutput (GPIOB, 21U, 0);
                GPIO_WritePinOutput (GPIOB, 20U, 0);

}


void Change_Accel_level(void)
{
int level=0;
if (xAngle>yAngle_Left)
{
if(xAngle>yAngle_Right)
	{
	level=xAngle/10;
	}
	else
		{level=yAngle_Right/10;}
}
else
{
	level=yAngle_Left/10;

}

if (level==1)
{
	//          GPIO_WritePinOutput (GPIOC, 3U, 0);
                GPIO_WritePinOutput (GPIOC, 5U, 0);
                GPIO_WritePinOutput (GPIOD, 1U, 0);
                GPIO_WritePinOutput (GPIOD, 2U, 0);
                GPIO_WritePinOutput (GPIOD, 3U, 0);
                GPIO_WritePinOutput (GPIOD, 4U, 0);
                GPIO_WritePinOutput (GPIOB, 21U, 0);
                GPIO_WritePinOutput (GPIOB, 20U, 1);
}
if (level==2)
{
	  //        GPIO_WritePinOutput (GPIOC, 3U, 0);
                GPIO_WritePinOutput (GPIOC, 5U, 0);
                GPIO_WritePinOutput (GPIOD, 1U, 0);
                GPIO_WritePinOutput (GPIOD, 2U, 0);
                GPIO_WritePinOutput (GPIOD, 3U, 0);
                GPIO_WritePinOutput (GPIOD, 4U, 0);
                GPIO_WritePinOutput (GPIOB, 21U, 1);
                GPIO_WritePinOutput (GPIOB, 20U, 1);
}

if (level==3)
{
	  //          GPIO_WritePinOutput (GPIOC, 3U, 0);
                GPIO_WritePinOutput (GPIOC, 5U, 0);
                GPIO_WritePinOutput (GPIOD, 1U, 1);
                GPIO_WritePinOutput (GPIOD, 2U, 0);
                GPIO_WritePinOutput (GPIOD, 3U, 0);
                GPIO_WritePinOutput (GPIOD, 4U, 0);
                GPIO_WritePinOutput (GPIOB, 21U, 1);
                GPIO_WritePinOutput (GPIOB, 20U, 1);
}
if (level==4)
{
	  //        GPIO_WritePinOutput (GPIOC, 3U, 0);
                GPIO_WritePinOutput (GPIOC, 5U, 1);
                GPIO_WritePinOutput (GPIOD, 1U, 1);
                GPIO_WritePinOutput (GPIOD, 2U, 0);
                GPIO_WritePinOutput (GPIOD, 3U, 0);
                GPIO_WritePinOutput (GPIOD, 4U, 0);
                GPIO_WritePinOutput (GPIOB, 21U, 1);
                GPIO_WritePinOutput (GPIOB, 20U, 1);
}
if (level==5)
{
	     //     GPIO_WritePinOutput (GPIOC, 3U,1);
                GPIO_WritePinOutput (GPIOC, 5U, 1);
                GPIO_WritePinOutput (GPIOD, 1U, 1);
                GPIO_WritePinOutput (GPIOD, 2U, 0);
                GPIO_WritePinOutput (GPIOD, 3U, 0);
                GPIO_WritePinOutput (GPIOD, 4U, 0);
                GPIO_WritePinOutput (GPIOB, 21U, 1);
                GPIO_WritePinOutput (GPIOB, 20U, 1);
}

if (level==6)
{
	       //   GPIO_WritePinOutput (GPIOC, 3U,1);
                GPIO_WritePinOutput (GPIOC, 5U, 1);
                GPIO_WritePinOutput (GPIOD, 1U, 1);
                GPIO_WritePinOutput (GPIOD, 2U, 1);
                GPIO_WritePinOutput (GPIOD, 3U, 0);
                GPIO_WritePinOutput (GPIOD, 4U, 0);
                GPIO_WritePinOutput (GPIOB, 21U, 1);
                GPIO_WritePinOutput (GPIOB, 20U, 1);
}
if (level==7)
{
	        //    GPIO_WritePinOutput (GPIOC, 3U,1);
                GPIO_WritePinOutput (GPIOC, 5U, 1);
                GPIO_WritePinOutput (GPIOD, 1U, 1);
                GPIO_WritePinOutput (GPIOD, 2U, 0);
                GPIO_WritePinOutput (GPIOD, 3U, 1);
                GPIO_WritePinOutput (GPIOD, 4U, 1);
                GPIO_WritePinOutput (GPIOB, 21U, 1);
                GPIO_WritePinOutput (GPIOB, 20U, 1);
}
if (level>=8)
{
	         //   GPIO_WritePinOutput (GPIOC, 3U,1);
                GPIO_WritePinOutput (GPIOC, 5U, 1);
                GPIO_WritePinOutput (GPIOD, 1U, 1);
                GPIO_WritePinOutput (GPIOD, 2U, 1);
                GPIO_WritePinOutput (GPIOD, 3U, 1);
                GPIO_WritePinOutput (GPIOD, 4U, 1);
                GPIO_WritePinOutput (GPIOB, 21U, 1);
                GPIO_WritePinOutput (GPIOB, 20U, 1);
}


//Func ends here//
}

void DIAL_SPIN_CW(void)
{

}
void DIAL_SPIN_CCW(void)
{

}

void RGY_OFF(void)
{

	GPIO_WritePinOutput (GPIOA, 5U, 0);
	GPIO_WritePinOutput (GPIOA, 12U, 0);
	GPIO_WritePinOutput (GPIOA, 13U, 0);
	GPIO_WritePinOutput (GPIOA, 14U, 0);
	GPIO_WritePinOutput (GPIOA, 17U, 0);
	GPIO_WritePinOutput (GPIOC, 7U, 0);
}

void GEAR_Change(void)

{



if (gear_counter>=6)
{gear_counter=6;}
if (gear_counter<=1)
{gear_counter=1;}

if (gear_counter==1)
{
	GPIO_WritePinOutput (GPIOA, 5U, 0);
		GPIO_WritePinOutput (GPIOA, 12U,1);
		GPIO_WritePinOutput (GPIOA, 13U, 0);
		GPIO_WritePinOutput (GPIOA, 14U, 0);
		GPIO_WritePinOutput (GPIOA, 17U, 0);
		GPIO_WritePinOutput (GPIOC, 7U, 0);

}
if (gear_counter==2)
{
	GPIO_WritePinOutput (GPIOA, 5U, 0);
		GPIO_WritePinOutput (GPIOA, 12U,1);
		GPIO_WritePinOutput (GPIOA, 13U, 1);
		GPIO_WritePinOutput (GPIOA, 14U, 0);
		GPIO_WritePinOutput (GPIOA, 17U, 0);
		GPIO_WritePinOutput (GPIOC, 7U, 0);

}
if (gear_counter==3)
{
	GPIO_WritePinOutput (GPIOA, 5U, 0);
		GPIO_WritePinOutput (GPIOA, 12U,1);
		GPIO_WritePinOutput (GPIOA, 13U, 1);
		GPIO_WritePinOutput (GPIOA, 14U, 0);
		GPIO_WritePinOutput (GPIOA, 17U, 1);
		GPIO_WritePinOutput (GPIOC, 7U, 0);

}
if (gear_counter==4)
{
	GPIO_WritePinOutput (GPIOA, 5U, 0);
		GPIO_WritePinOutput (GPIOA, 12U,1);
		GPIO_WritePinOutput (GPIOA, 13U, 1);
		GPIO_WritePinOutput (GPIOA, 14U, 1);
		GPIO_WritePinOutput (GPIOA, 17U, 1);
		GPIO_WritePinOutput (GPIOC, 7U, 0);

}
if (gear_counter==5)
{
	GPIO_WritePinOutput (GPIOA, 5U, 0);
		GPIO_WritePinOutput (GPIOA, 12U,1);
		GPIO_WritePinOutput (GPIOA, 13U, 1);
		GPIO_WritePinOutput (GPIOA, 14U, 1);
		GPIO_WritePinOutput (GPIOA, 17U, 1);
		GPIO_WritePinOutput (GPIOC, 7U, 1);

}
if (gear_counter==6)
{
	GPIO_WritePinOutput (GPIOA, 5U, 1);
		GPIO_WritePinOutput (GPIOA, 12U,1);
		GPIO_WritePinOutput (GPIOA, 13U, 1);
		GPIO_WritePinOutput (GPIOA, 14U, 1);
		GPIO_WritePinOutput (GPIOA, 17U, 1);
		GPIO_WritePinOutput (GPIOC, 7U, 1);

}


// Gear Func stops here
}



void Delay(uint32_t n)
{
	  SysTick->CTRL |= SysTick_CTRL_CLKSOURCE_Msk;                      /* use core clock */
	    SysTick->CTRL &= ~SysTick_CTRL_TICKINT_Msk;                       /* disable interrupt */
	    SysTick->LOAD = n * ((CLOCK_GetFreq(kCLOCK_CoreSysClk)) / 1000U); /* n ms */
	    SysTick->VAL = 0U;                                                /* clear COUNTFLAG */

	    SysTick->CTRL |= SysTick_CTRL_ENABLE_Msk;

	    /* wait for timeout */
	    while (0U == (SysTick->CTRL & SysTick_CTRL_COUNTFLAG_Msk))
	    {
	    }

	    SysTick->CTRL &= ~SysTick_CTRL_ENABLE_Msk;
}

void Update_Angles(void)
{
    /* Get new accelerometer data. */
        FXOS_ReadSensorData(&fxosHandle, &sensorData);

        /* Get the X and Y data from the sensor data structure.fxos_data */
        xData = (int16_t)((uint16_t)((uint16_t)sensorData.accelXMSB << 8) | (uint16_t)sensorData.accelXLSB);
        yData = (int16_t)((uint16_t)((uint16_t)sensorData.accelYMSB << 8) | (uint16_t)sensorData.accelYLSB);

        // Convert raw data to angle ( -90:0:90 degrees).

        //=======xData and yData Swapped ===========///
        xAngle = (int16_t)floor((double)yData * 0.011);
      //===================Changed Here ==================
        if (xAngle >= 0)
                {
        	xAngle_Gear=xAngle;
        	xAngle=0;

                }
        if (xAngle < 0)
        {

            xAngle *= -1;
            xAngle_Gear=0;


        }
        yAngle_Left = (int16_t)floor((double)xData * 0.011);
        if (yAngle_Left < 0)
        {
        //    yAngle *= -1;
        	yAngle_Right=yAngle_Left*(-1);
        	yAngle_Left=0;
        }
}

void Send_Car_Cmd(void)

{
if(xAngle>5)
	{
	if ((xAngle+10>yAngle_Left)&(xAngle+10>yAngle_Right))
			{
		power_level=xAngle*gear_counter/2;
		car_cmd[0]='B';
		car_cmd[1]=0x30+(power_level/100);
		car_cmd[2]=0x30+((power_level%100) /10);
		car_cmd[3]=0x30+((power_level%10) /1);
		car_cmd[4]='\n';

	    LPUART_WriteBlocking(BT_LPUART, car_cmd, sizeof(car_cmd) - 1);
			}
	 if((yAngle_Left)<(yAngle_Right))
	{
		power_level=yAngle_Right*gear_counter/2;
		car_cmd[0]='R';
			car_cmd[1]=0x30+(power_level/100);
			car_cmd[2]=0x30+((power_level%100) /10);
			car_cmd[3]=0x30+((power_level%10) /1);
			car_cmd[4]='\n';

		    LPUART_WriteBlocking(BT_LPUART, car_cmd, sizeof(car_cmd) - 1);

	}

	if((yAngle_Left)>(yAngle_Right))
		{
		power_level=yAngle_Left*gear_counter/2;
		car_cmd[0]='G';
		car_cmd[1]=0x30+(power_level/100);
		car_cmd[2]=0x30+((power_level%100) /10);
		car_cmd[3]=0x30+((power_level%10) /1);
		car_cmd[4]='\n';

	    LPUART_WriteBlocking(BT_LPUART, car_cmd, sizeof(car_cmd) - 1);
}

	}



}


//==========================================================//

int main(void)
{

    i2c_master_config_t i2cConfig = {0};
    uint32_t i2cSourceClock = 0;

    // =================Changed HERE for 3 PWM ======================//




    uint8_t i = 0;
    uint8_t regResult = 0;
    uint8_t array_addr_size = 0;
    bool foundDevice = false;

    i2cSourceClock = CLOCK_GetFreq(ACCEL_I2C_CLK_SRC);
    fxosHandle.base = BOARD_ACCEL_I2C_BASEADDR;
    fxosHandle.i2cHandle = &g_MasterHandle;


    /*=======================================================
     * Changes made here
     *
     * ======================================================
     */
    gpio_pin_config_t pin_config =

        {
        kGPIO_DigitalOutput,1,
        };

    // LPUART (BT Config)
    lpuart_config_t config_lpuart;


    //=========================================================//
    /* Board pin, clock, debug console init */
    BOARD_InitPins();
    BOARD_BootClockRUN();
    BOARD_InitDebugConsole();
    CLOCK_SetLpuartClock(2U);
    LPUART_GetDefaultConfig(&config_lpuart);


    /*=======================================================
     * Changes made here
     *

     * ======================================================
     */

    config_lpuart.baudRate_Bps = 115200;
    config_lpuart.enableTx = true;
    config_lpuart.enableRx = true;

    LPUART_Init(BT_LPUART, &config_lpuart, CLOCK_GetFreq(LPUART_CLKSRC));



  //  	GPIO_PinInit(GPIOC, 3U, &pin_config);
        GPIO_PinInit(GPIOC, 5U, &pin_config);
        GPIO_PinInit(GPIOB, 21U, &pin_config);
        GPIO_PinInit(GPIOB, 20U, &pin_config);
        GPIO_PinInit(GPIOD, 1U, &pin_config);
        GPIO_PinInit(GPIOD, 2U, &pin_config);
        GPIO_PinInit(GPIOD, 3U, &pin_config);
        GPIO_PinInit(GPIOD, 4U, &pin_config);

        //=========================================================//
        GPIO_PinInit(GPIOA, 5U, &pin_config);
           GPIO_PinInit(GPIOA, 12U, &pin_config);
           GPIO_PinInit(GPIOA, 13U, &pin_config);
           GPIO_PinInit(GPIOA, 14U, &pin_config);
           GPIO_PinInit(GPIOA, 17U, &pin_config);
           GPIO_PinInit(GPIOC, 7U, &pin_config);
       RGY_OFF();

    /*
     * i2cConfig.baudRate_Bps = 100000U;
     * i2cConfig.enableHighDrive = false;
     * i2cConfig.enableStopHold = false;
     * i2cConfig.glitchFilterWidth = 0U;
     * i2cConfig.enableMaster = true;
     */
    I2C_MasterGetDefaultConfig(&i2cConfig);
    I2C_MasterInit(BOARD_ACCEL_I2C_BASEADDR, &i2cConfig, i2cSourceClock);
    I2C_MasterTransferCreateHandle(BOARD_ACCEL_I2C_BASEADDR, &g_MasterHandle, NULL, NULL);

    /* Find sensor devices */
    array_addr_size = sizeof(g_accel_address) / sizeof(g_accel_address[0]);
    for (i = 0; i < array_addr_size; i++)
    {
        fxosHandle.xfer.slaveAddress = g_accel_address[i];
        if (FXOS_ReadReg(&fxosHandle, WHO_AM_I_REG, &regResult, 1) == kStatus_Success)
        {
            foundDevice = true;
            break;
        }
        if ((i == (array_addr_size - 1)) && (!foundDevice))
        {
        //    PRINTF("\r\nDo not found sensor device\r\n");
        	// GLOW AN LED HERE
            while (1)
            {
            };
        }
    }

    /* Init accelerometer sensor */
    FXOS_Init(&fxosHandle);
    /* Init timer */
    Timer_Init();

   //* Print a note to terminal */
 //   PRINTF("\r\nWelcome to BUBBLE example\r\n");
//    PRINTF("\r\nYou will see the change of LED brightness when change angles of board\r\n");

    // reset Gear dial


    /* Main loop. Get sensor data and update duty cycle */


    while (1)
    {
    	Update_Angles();
    	Board_UpdatePwm(xAngle, yAngle_Left,yAngle_Right);
    	Change_Accel_level();
    	Send_Car_Cmd();
    	 for(int x=0;x<16;x++){Delay(1000000);}







// Gear Control Here

        while(xAngle_Gear>0)
        {
        	DIAL_OFF();
        		power_level=0;
        		car_cmd[0]='S';
        			car_cmd[1]=0x30;
        			car_cmd[2]=0x30;
        			car_cmd[3]=0x30;
        			car_cmd[4]='\n';
        			   LPUART_WriteBlocking(BT_LPUART, car_cmd, sizeof(car_cmd) - 1);
        		    	 for(int x=0;x<16;x++){Delay(1000000);}

        			   if(xAngle_Gear>50)
        			   {
        						power_level=yAngle_Left*gear_counter/2;
        						car_cmd[0]='O';
        						car_cmd[1]=0x30+(power_level/100);
        						car_cmd[2]=0x30+((power_level%100) /10);
        						car_cmd[3]=0x30+((power_level%10) /1);
        						car_cmd[4]='\n';
        					    LPUART_WriteBlocking(BT_LPUART, car_cmd, sizeof(car_cmd) - 1);

        				}


        		    	 for(int x=0;x<16;x++){Delay(1000000);}

        	// Car is stop for gear change

        	Update_Angles();
        	 Board_UpdatePwm(xAngle, yAngle_Left,yAngle_Right);
                  while(yAngle_Left>30)
                  {
                	  gear_counter++;
                	  GEAR_Change();
                	  Update_Angles();
                	  Board_UpdatePwm(xAngle, yAngle_Left,yAngle_Right);
                	  for(int x=0;x<6;x++){Delay(1000000);}
                  }

                  while(yAngle_Right>30)
                                    {
                	  gear_counter--;
                	  GEAR_Change();
                	  Update_Angles();
                	  Board_UpdatePwm(xAngle, yAngle_Left,yAngle_Right);
                	  for(int x=0;x<6;x++){Delay(1000000);}

                                    }

                  }
        }




        /* Print out the raw accelerometer data. */
      //  PRINTF("x= %d y = %d\r\n", xData, yData);

   // infinity loop ends here
    }