Edge Detecting Bot using XENSIV™ BGT60UTR11AIP radar

# ===========================================================================
# Copyright (C) 2021-2022 Infineon Technologies AG
#
# Redistribution and use in source and binary forms, with or without
# modification, are permitted provided that the following conditions are met:
#
# 1. Redistributions of source code must retain the above copyright notice,
#    this list of conditions and the following disclaimer.
# 2. 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.
# 3. Neither the name of the copyright holder 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.
# ===========================================================================

# Edge detecting bot source code- Infineon Developer Community

import argparse

from ifxradarsdk import get_version_full
from ifxradarsdk.fmcw import DeviceFmcw
from ifxradarsdk.fmcw.types import FmcwSimpleSequenceConfig, FmcwMetrics
from helpers.DistanceAlgo import *

import RPi.GPIO as GPIO
from time import sleep

in1=10              #Define GPIO pins
in2=9
in3=7
in4=8
in5=6
in6=5

GPIO.setmode(GPIO.BCM)      #diable gpio warnings
GPIO.setwarnings(False)

GPIO.setup(in1,GPIO.OUT)        #setup gpio pins
GPIO.setup(in2,GPIO.OUT)
GPIO.setup(in3,GPIO.OUT)
GPIO.setup(in4,GPIO.OUT)
GPIO.setup(in5,GPIO.OUT)
GPIO.setup(in6,GPIO.OUT)

GPIO.output(in1,GPIO.LOW)       #make all pins low after init
GPIO.output(in2,GPIO.LOW)
GPIO.output(in3,GPIO.LOW)
GPIO.output(in4,GPIO.LOW)
GPIO.output(in5,GPIO.LOW)
GPIO.output(in6,GPIO.LOW)
# -------------------------------------------------
#Motor control functions
def stopAll():
    GPIO.output(in1,GPIO.LOW)
    GPIO.output(in2,GPIO.LOW)
    GPIO.output(in3,GPIO.LOW)
    GPIO.output(in4,GPIO.LOW)
    GPIO.output(in5,GPIO.LOW)
    GPIO.output(in6,GPIO.LOW)


def goFront(t):
    GPIO.output(in2,GPIO.HIGH)
    GPIO.output(in5,GPIO.HIGH)
    if(t>0):
        sleep(t)
        stopAll()
    
def goBack(t):
    GPIO.output(in1,GPIO.HIGH)
    GPIO.output(in6,GPIO.HIGH)
    sleep(t)
    stopAll()
    
def turnAClk(t):
    GPIO.output(in1,GPIO.HIGH)
    GPIO.output(in3,GPIO.HIGH)
    GPIO.output(in5,GPIO.HIGH)
    sleep(t)
    stopAll()

def turnClk(t):
    GPIO.output(in2,GPIO.HIGH)
    GPIO.output(in4,GPIO.HIGH)
    GPIO.output(in6,GPIO.HIGH)
    sleep(t)
    stopAll()

# -------------------------------------------------
# Helpers
def parse_program_arguments(description, def_nframes, def_frate):
    # Parse all program attributes
    # description:   describes program
    # def_nframes:   default number of frames
    # def_frate:     default frame rate in Hz

    parser = argparse.ArgumentParser(description=description)
    parser.add_argument('-n', '--nframes', type=int,
                        default=def_nframes, help="number of frames, default " + str(def_nframes))
    parser.add_argument('-f', '--frate', type=int, default=def_frate,
                        help="frame rate in Hz, default " + str(def_frate))
    return parser.parse_args()

# Process radar frame and return peak distance    
def getRadarDistancePeak( frame ):
    frame_data = frame[0]
    antenna_samples = frame_data[i_ant, :, :]
    distance_peak, _ = algo.compute_distance(antenna_samples)
    return distance_peak

# Stop, reverse and turn the robot.
def reverseAndTurn():
    stopAll()
    goBack(0.5)
    turnClk(0.15)

# -------------------------------------------------
# Main logic
# -------------------------------------------------
if __name__ == '__main__':

    args = parse_program_arguments(
        '''Displays distance plot from Radar Data''',
        def_nframes=0,
        def_frate=75)
    
    device1 = DeviceFmcw(uuid="00313853-314c-4c35-3135-303039303137")
    device2 = DeviceFmcw(uuid="00313853-3959-3434-3034-303331303335")
    
    #device1= DeviceFmcw(port="COM6")
    #device2= DeviceFmcw(port="COM19")
    
    i_ant = 0  # use only 1st RX antenna
    num_rx_antennas = 1

    metrics = FmcwMetrics(
        range_resolution_m=0.05,
        max_range_m=2,
        max_speed_m_s=3,
        speed_resolution_m_s=0.2,
        center_frequency_Hz=60_750_000_000,
    )

    # create acquisition sequence based on metrics parameters
    sequence = device1.create_simple_sequence(FmcwSimpleSequenceConfig())
    sequence = device2.create_simple_sequence(FmcwSimpleSequenceConfig())
    sequence.loop.repetition_time_s = 1 / args.frate  # set frame repetition time

    # convert metrics into chirp loop parameters
    chirp_loop = sequence.loop.sub_sequence.contents
    device1.sequence_from_metrics(metrics, chirp_loop)
    device2.sequence_from_metrics(metrics, chirp_loop)
    
    # set remaining chirp parameters which are not derived from metrics
    chirp = chirp_loop.loop.sub_sequence.contents.chirp
    chirp.sample_rate_Hz = 1_000_000
    chirp.rx_mask = (1 << num_rx_antennas) - 1
    chirp.tx_mask = 1
    chirp.tx_power_level = 31
    chirp.if_gain_dB = 33
    chirp.lp_cutoff_Hz = 500000
    chirp.hp_cutoff_Hz = 80000

    device1.set_acquisition_sequence(sequence)
    device2.set_acquisition_sequence(sequence)

    algo = DistanceAlgo(chirp, chirp_loop.loop.num_repetitions)
    print("Program started")
    
    while(1):
      # Move robot forward
      goFront(0)
  
      # Get distance data from radar sensor 1
      frame_contents = device1.get_next_frame()
      distance_peak = getRadarDistancePeak( frame_contents )
      print("Distance 1:" + format(distance_peak, "^05.3f") + "m")
      
      # Reverse and turn the robot if table corner is detected
      if(distance_peak > 0.25):
          reverseAndTurn()
          device1.stop_acquisition()
          device2.stop_acquisition()
  
      # Get distance data from radar sensor 2
      frame_contents = device2.get_next_frame()
      distance_peak = getRadarDistancePeak( frame_contents )
      print("Distance 2:" + format(distance_peak, "^05.3f") + "m")
      
      # Reverse and turn the robot if table corner is detected
      if(distance_peak > 0.25):
          reverseAndTurn()
          device1.stop_acquisition()
          device2.stop_acquisition()
  
      print("Still Running")