Obstacle Avoidance Robot Using I.MX93 Cortex-M33

/*
 * Copyright (c) 2015, Freescale Semiconductor, Inc.
 * Copyright 2016-2021 NXP
 * All rights reserved.
 *
 * SPDX-License-Identifier: BSD-3-Clause
 */

#include "fsl_debug_console.h"
#include "board.h"
#include "app.h"
#include "fsl_tpm.h"
#include "fsl_rgpio.h"
#include "fsl_lpuart.h"
#include "Navigation.h"

/*******************************************************************************
 * Definitions
 ******************************************************************************/

/*******************************************************************************
 * Prototypes
 ******************************************************************************/
void GPIO_SETUP(void);
void FORWARD(void);
void BACKWARD(void);
void TURN_RIGHT(void);
void TURN_LEFT(void);
void HALT(void);
void OBJECT_AVOID(void);
/*******************************************************************************
 * Variables
 ******************************************************************************/
volatile bool tpmIsrFlag = false;
volatile uint32_t risingTime = 0;
volatile uint32_t fallingTime = 0;
volatile bool gotRisingEdge = false;

/*******************************************************************************
 * Code
 ******************************************************************************/
/*!
 * @brief Init_LPUART function
 */
void Init_LPUART(void)
{
    lpuart_config_t config;

    /* Get default UART config (8N1, no parity, no flow control) */
    LPUART_GetDefaultConfig(&config);

    /* Modify parameters as needed */
    config.baudRate_Bps = 115200U;
    config.enableTx = true;
    config.enableRx = true;

    /* Initialize the UART with the specified parameters */
    LPUART_Init(BOARD_LPUART, &config, BOARD_LPUART_CLK_FREQ);
}

/*!
 * @brief CalculateDistance function
 */
float CalculateDistance(void)
{
    uint32_t ticks;
    if (fallingTime >= risingTime)
        ticks = fallingTime - risingTime;
    else
        ticks = (TPM_MAX_COUNTER_VALUE(DEMO_TPM_BASEADDR) - risingTime) + fallingTime; /* Overflow handled */

    float tpmClk = CLOCK_GetIpFreq(LPTPM_CLOCK_ROOT); /* You may adjust to TPM clock */
    float timeUs = ((ticks * 1.0f) / tpmClk) * 1e6;
    PRINTF("TPM Clock : %.2f\r\n",tpmClk);
    PRINTF("TIME IN US FROM ECHO : %.2f\r\n",timeUs);

    float distanceCm = timeUs / 58.0f; /* Convert Micro second into centimeter(distance) */

    return distanceCm;
}

/*!
 * @brief SetUp function
 */
void SetUp(void)
{
    rgpio_pin_config_t out_config ={
        kRGPIO_DigitalOutput,
        0,
    };
    
    /* Trigger pin */
    RGPIO_PinInit(TRIG_RGPIO,TRIG_PIN,&out_config);
    RGPIO_PinWrite(TRIG_RGPIO,TRIG_PIN,0);

    RGPIO_PinInit(MOTOR_RGPIO, A1, &out_config);
    RGPIO_PinInit(MOTOR_RGPIO, A2, &out_config);
    RGPIO_PinInit(MOTOR_RGPIO, B1, &out_config);
    RGPIO_PinInit(MOTOR_RGPIO, B2, &out_config);

}

/*!
 * @brief SendTrigPulse function
 */
void SendTrigPulse(void)
{
    RGPIO_PinWrite(TRIG_RGPIO, TRIG_PIN, 0);
    SDK_DelayAtLeastUs(2, SystemCoreClock); 
    RGPIO_PinWrite(TRIG_RGPIO, TRIG_PIN, 1);
    SDK_DelayAtLeastUs(10, SystemCoreClock); 
    RGPIO_PinWrite(TRIG_RGPIO, TRIG_PIN, 0);
}

/*!
 * @brief TPM_INPUT_CAPTURE_HANDLER function
 */
void TPM_INPUT_CAPTURE_HANDLER(void)
{
    uint32_t status = TPM_GetStatusFlags(DEMO_TPM_BASEADDR);

    if (status & TPM_CHANNEL_FLAG)
    {
        uint32_t capturedValue = TPM_GetChannelValue(DEMO_TPM_BASEADDR, BOARD_TPM_INPUT_CAPTURE_CHANNEL);

        if (!gotRisingEdge)
        {
            risingTime = capturedValue;
            gotRisingEdge = true;

            /* Switch to falling edge detection */
            TPM_SetupInputCapture(DEMO_TPM_BASEADDR,BOARD_TPM_INPUT_CAPTURE_CHANNEL,kTPM_FallingEdge);
        }
        else
        {
            fallingTime = capturedValue;
            tpmIsrFlag = true;
            gotRisingEdge = false;

            /* Switch back to rising edge detection for next measurement */
            TPM_SetupInputCapture(DEMO_TPM_BASEADDR,BOARD_TPM_INPUT_CAPTURE_CHANNEL,kTPM_RisingEdge);
        }

        /* Clear interrupt flag for this channel */
        TPM_ClearStatusFlags(DEMO_TPM_BASEADDR, TPM_CHANNEL_FLAG);
    }

    SDK_ISR_EXIT_BARRIER;
}

/*!
 * @brief OBJECT_AVOID function
 */
void OBJECT_AVOID(void)
{
    float distance = CalculateDistance();
    PRINTF("Distance: %.2f cm\r\n", distance);
    
    if (distance >= 20)
    {
        FORWARD();
    }
    else if (distance <= 19)
    {
        HALT();
        SDK_DelayAtLeastUs(500000,SystemCoreClock);
        BACKWARD();
        SDK_DelayAtLeastUs(500000,SystemCoreClock);
        HALT();
        SDK_DelayAtLeastUs(500000,SystemCoreClock);
        TURN_RIGHT();
        SDK_DelayAtLeastUs(500000,SystemCoreClock);
    }
}

/*!
 * @brief Main function
 */
int main(void)
{
    tpm_config_t tpmInfo;

    SetUp();  /* Set up the GPIO */

    /* Board pin, clock, debug console init */
    BOARD_InitHardware();

    // Init_LPUART(); /* Initialise one more UART */

    /* Print a note to terminal */
    PRINTF("\r\nUltrasonic sensor demo\r\n");
    PRINTF("\n");
    
    TPM_GetDefaultConfig(&tpmInfo);

    /* Initialize TPM module */
    TPM_Init(DEMO_TPM_BASEADDR, &tpmInfo);

    /* Setup input capture on a TPM channel */
    TPM_SetupInputCapture(DEMO_TPM_BASEADDR, BOARD_TPM_INPUT_CAPTURE_CHANNEL, kTPM_RisingEdge);

    /* Set the timer to be in free-running mode */
    TPM_SetTimerPeriod(DEMO_TPM_BASEADDR, TPM_MAX_COUNTER_VALUE(DEMO_TPM_BASEADDR));

    /* Enable channel interrupt when the second edge is detected */
    TPM_EnableInterrupts(DEMO_TPM_BASEADDR, TPM_CHANNEL_INTERRUPT_ENABLE);

    /* Enable at the NVIC */
    EnableIRQ(TPM_INTERRUPT_NUMBER);

    TPM_StartTimer(DEMO_TPM_BASEADDR, kTPM_SystemClock);

    while (1)
    {
        tpmIsrFlag = false;

        SendTrigPulse(); /* Send 10us pulse to the sensor to start measuring */
        
        while (tpmIsrFlag != true) /* When both edge captured flag is true */
        {
        }

        OBJECT_AVOID(); /* Oject detection based on the ultrasonic sensor */

        // SDK_DelayAtLeastUs(600000,SystemCoreClock);
    }
}