Published August 30, 2024 © GPL3+

StickandSee: An Intelligent Blind Walking Stick Assistant

StickandSee helps people with visual impairment walk on the street by giving notification of obstacle in front of them

IntermediateShowcase (no instructions)Over 1 day418

StickandSee: An Intelligent Blind Walking Stick Assistant

Things used in this project

Hardware components

Seeed Studio XIAO ESP32S3 Sense

Seeed Studio Grove Shield for Seeeduino XIAO - with embedded battery management chip

DFRobot UNIHIKER M10 - IoT Python Programming Single Board Computer with Touchscreen

Software apps and online services

Arduino IDE

Edge Impulse Studio

Seeed Studio SenseCraft-Web-Toolkit

Story

ProblemAccording to the Central Statistics Agency, people with visual impairments in Indonesia are approximately 8.36%. With the presence of visually impaired people around us, guiding blocks or tactile guidance have been used to help visually impaired people walk on the sidewalk easily. But, in some places, guiding blocks are blocked by other objects or obstacles, such as cars, motorcycles, or other objects, so that visually impaired people can't walk comfortably along the guiding blocks on the sidewalk.

Some pictures below visualize the condition of blocked guiding blocks in Indonesia, which can disturb people with visual impairments who are unable to walk safely because of cars or other objects blocking the guiding blocks.

source: https://news.detik.com/berita/d-6624117/savesijalurkuning-kenapa-lindungi-guiding-block-meski-tunanetra-sedikit

source: https://news.detik.com/berita/d-6656803/savejalurkuning-baru-3-titik-guiding-block-di-jl-dewi-sartika-dibereskan

Based on the pictures above, comfort and safety aspects for people with visual impairments are important to strive for because they have chances to live a decent life, and one thing they need to get is a feeling of comfort when walking on the sidewalk. From the problems explained above, this project created an embedded system to detect obstacles in front of blind people so that they can anticipate walking on the sidewalk.

Our IdeaBased on the problem explained in the ProblemSection, this project created an electronic system that was applied to a blind stick for visually impaired people. The system that was created has the ability based on artificial intelligence to detect guiding blocks and obstacles in front of visually impaired people. the system that has been created is called StickandSee.

What is StickandSee?StickandSeeis an electronic system utilizing embedded artificial intelligence inside it using the platform XIAO ESP32S3, which has a camera module. The system aims to detect guiding blocks and obstacles in front of visually impaired people. This tool will be installed on the visually impaired person's walking stick.

How StickandSee Work?There are several processes and steps carried out by StickandSeeto help provide information in the form of notifications to visually impaired people whether there are obstacles ahead or not. The processes carried out by StickandSee include:1. Collecting datasets2. Labeling object3. Training dataset4. Object detectiontesting

1.CollectingDatasetThis process is carried out by taking photos of the objects to be detected, such as guiding blocks, cars, and motorcycles. Taking pictures can be done using the camera on the XIAO ESP32S3 and can also be done by using a cellphone camera. In this process, we used a smartphone camera to take images of objects. The steps taken in the photo-taking process include:1. Taking object pictures using a phone camera with square photo mode2. Resize the image to 320 x 320 pixels using MS Paint

example of 320x320 pixel guiding block image taken by phone camera

2.Labeling ObjectThis process aims to label the object inside the picture. The labeling process is carried out by using Edge Impulse Studio. The steps taken in this process include:

Login to the Edge Impulse Studio website
Click the "Create new project" button in the up-right corner

Create new project

Enter a project name
In the left tab, choose Dashboard, and click "Add existing data"

Add existing data

In the opened dialog box, click "Uploaddata"

Upload data

Select the data to be labeled during the labeling process (you can select afolder or files during the upload data process and it will automatically split trainingdata and testingdata)

Upload data

Your image should appear like this with the labels collumn filled with "-" at first

Select each image to create a label in the object by clicking 3 dots and choosing the "Edit labels" menu

an image window should appear and you can hold left-click and drag on the object to label it, give it a name, and click the "Save labels" button

Object labeling process

You should label the entire images in the "training" and "testing" tab until the percentage gives you a proportion of 80% training and 20% testing labeled dataset
For other references, you can read from the tutorial in this link provided by MJRoBot (Marcelo Rovai)

3.TrainingDatasetAfter the labeling process, the next section is to train the labeled object. In this process, there are several steps to be taken by configuring some parameters and training the data.

the left tab, click "Create Impulse" as can be seen in the picture below. For the first trial, we just used to detect one kind of object, which is the "Tactile Guide".

Creating Impulse

Click "Save Impulse"
In the left tab, click "image". This process will generate the feature of the image. We chose the grayscale image to speed up the process and don't forget to click the "Save Parameters" button as can be seen in the picture below.

Dataset pre-processing

After we save the parameter, it will move to the "Generate Features" tab automatically and we can click the "Generate Features" button so that the result can be seen in the picture below

Feature generation of the dataset

Next step is to perform training the dataset. In the left tab, click the "object detection" menu. In this step, we set the number of training cycles to 150 and validation set size to 30. We use FOMO MobileNet V2.035 as the model with 1 output class as the "Tactile Guide". Click the "start" button to start the training process.

Training result

4.Object DetectionTestingIn this process, we test object detection with 3 conditions.1. Object detection by using dataset image2. Object detection by using images from Google3. Object detection with Arduino code generated from edge impulse

Object detection by using data set image (video demonstration)

Object detection by using images from Google (video demonstration)

Object detection using Arduino IDE

Car and Motorcycle Detection

CameraStabilizationusing STM32F411CEU6from ARM

Stickandsee uses STM32F411CEU6 supported by ARM to perform a stabilization mechanism so that the position of XIAO ESP32's stays in its position. The stabilization mechanism using PID control so that it can move the servo smoothly to stabilize the camera position.

FallDetectionusing UNIHIKER

Stickandsee uses UNIHIKER supported by DFROBOT to perform a fall detection notification that comes from a buzzer inside UNIHIKER. The buzzer will emit a notification sound if it is in a flat position which is considered to be the position where the blind stick has fallen.

StickandSee

/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2024 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "stdio.h"
#include "fonts.h"
#include "ssd1306.h"
#include "test.h"
#include "String.h"
#include "stdio.h"
#include "stdlib.h"

#include "IOI2C.h"
#include "MPU6050.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */

/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
#define LED_ON 0
#define LED_OFF 1
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/
I2C_HandleTypeDef hi2c1;
I2C_HandleTypeDef hi2c2;
I2C_HandleTypeDef hi2c3;

TIM_HandleTypeDef htim3;

UART_HandleTypeDef huart1;
UART_HandleTypeDef huart2;
UART_HandleTypeDef huart6;

/* USER CODE BEGIN PV */
float roll;
float pitch;
float yaw;

float char_roll[10];
float char_pitch[10];
float char_yaw[10];

int16_t gy25_roll;
int16_t gy25_pitch;
int16_t gy25_yaw;

int char_gy25_roll;
int char_gy25_pitch;
int char_gy25_yaw;

int pilih = 0;
int menu = 0;
/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_I2C1_Init(void);
static void MX_I2C2_Init(void);
static void MX_I2C3_Init(void);
static void MX_USART1_UART_Init(void);
static void MX_USART2_UART_Init(void);
static void MX_USART6_UART_Init(void);
static void MX_TIM3_Init(void);
/* USER CODE BEGIN PFP */
int PB_OK();
int PB_CANCEL();
int PB_UP();
int PB_DOWN();

void setLED(int state);
void blinkLED(int delay);

void getIMUData(void);
void getCompassData(void);

void menuHomeScreen(void);
void menuShowIMU(void);
void menuShowCompass(void);
void menuShowButton(void);
void menuRunProgram(void);

void setServoPosition(int position);
void setServoTimerPulse(int timerValue);
long map(long x, long in_min, long in_max, long out_min, long out_max);
void servoSweep();

void PIDStabilize(void);

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */

/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{
  /* USER CODE BEGIN 1 */

  /* USER CODE END 1 */

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_I2C1_Init();
  MX_I2C2_Init();
  MX_I2C3_Init();
  MX_USART1_UART_Init();
  MX_USART2_UART_Init();
  MX_USART6_UART_Init();
  MX_TIM3_Init();
  /* USER CODE BEGIN 2 */


  IIC_Init(hi2c3);
  SSD1306_Init();
  setLED(0);

  SSD1306_GotoXY(0, 0);
  SSD1306_Puts("Author:Widayaka", &Font_7x10, 1);
  SSD1306_GotoXY(0, 10);
  SSD1306_Puts("Project:Master", &Font_7x10, 1);
  SSD1306_GotoXY(0, 20);
  SSD1306_Puts("Month:July", &Font_7x10, 1);
  SSD1306_GotoXY(0, 30);
  SSD1306_Puts("Year:2024", &Font_7x10, 1);
  SSD1306_GotoXY(0, 40);
  SSD1306_Puts("Dhurobotic", &Font_7x10, 1);

  SSD1306_UpdateScreen();
  HAL_Delay(1000);
  SSD1306_Clear();

  HAL_TIM_PWM_Start(&htim3, TIM_CHANNEL_1);
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  printf("\n\rMPU6050 is initializing...\n\r");
  MPU6050_initialize();
  printf("\n\rMPU6050 has been initialized...\n\r");
  MPU6050_DMPInit();



  while (1)
  {
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
//	  while (menu==0){menuHomeScreen();}
//	  while (menu==1){menuShowIMU();}
//	  while (menu==2){menuShowCompass();}
//	  while (menu==3){menuShowButton();}
	  PIDStabilize();
//	  setServoPosition(90);
//	  blinkLED(100);
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Configure the main internal regulator output voltage
  */
  __HAL_RCC_PWR_CLK_ENABLE();
  __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE2);

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLM = 25;
  RCC_OscInitStruct.PLL.PLLN = 168;
  RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
  RCC_OscInitStruct.PLL.PLLQ = 4;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV4;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  {
    Error_Handler();
  }
}

/**
  * @brief I2C1 Initialization Function
  * @param None
  * @retval None
  */
static void MX_I2C1_Init(void)
{

  /* USER CODE BEGIN I2C1_Init 0 */

  /* USER CODE END I2C1_Init 0 */

  /* USER CODE BEGIN I2C1_Init 1 */

  /* USER CODE END I2C1_Init 1 */
  hi2c1.Instance = I2C1;
  hi2c1.Init.ClockSpeed = 400000;
  hi2c1.Init.DutyCycle = I2C_DUTYCYCLE_2;
  hi2c1.Init.OwnAddress1 = 0;
  hi2c1.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
  hi2c1.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
  hi2c1.Init.OwnAddress2 = 0;
  hi2c1.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
  hi2c1.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
  if (HAL_I2C_Init(&hi2c1) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN I2C1_Init 2 */

  /* USER CODE END I2C1_Init 2 */

}

/**
  * @brief I2C2 Initialization Function
  * @param None
  * @retval None
  */
static void MX_I2C2_Init(void)
{

  /* USER CODE BEGIN I2C2_Init 0 */

  /* USER CODE END I2C2_Init 0 */

  /* USER CODE BEGIN I2C2_Init 1 */

  /* USER CODE END I2C2_Init 1 */
  hi2c2.Instance = I2C2;
  hi2c2.Init.ClockSpeed = 400000;
  hi2c2.Init.DutyCycle = I2C_DUTYCYCLE_2;
  hi2c2.Init.OwnAddress1 = 0;
  hi2c2.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
  hi2c2.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
  hi2c2.Init.OwnAddress2 = 0;
  hi2c2.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
  hi2c2.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
  if (HAL_I2C_Init(&hi2c2) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN I2C2_Init 2 */

  /* USER CODE END I2C2_Init 2 */

}

/**
  * @brief I2C3 Initialization Function
  * @param None
  * @retval None
  */
static void MX_I2C3_Init(void)
{

  /* USER CODE BEGIN I2C3_Init 0 */

  /* USER CODE END I2C3_Init 0 */

  /* USER CODE BEGIN I2C3_Init 1 */

  /* USER CODE END I2C3_Init 1 */
  hi2c3.Instance = I2C3;
  hi2c3.Init.ClockSpeed = 400000;
  hi2c3.Init.DutyCycle = I2C_DUTYCYCLE_2;
  hi2c3.Init.OwnAddress1 = 0;
  hi2c3.Init.AddressingMode = I2C_ADDRESSINGMODE_7BIT;
  hi2c3.Init.DualAddressMode = I2C_DUALADDRESS_DISABLE;
  hi2c3.Init.OwnAddress2 = 0;
  hi2c3.Init.GeneralCallMode = I2C_GENERALCALL_DISABLE;
  hi2c3.Init.NoStretchMode = I2C_NOSTRETCH_DISABLE;
  if (HAL_I2C_Init(&hi2c3) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN I2C3_Init 2 */

  /* USER CODE END I2C3_Init 2 */

}

/**
  * @brief TIM3 Initialization Function
  * @param None
  * @retval None
  */
static void MX_TIM3_Init(void)
{

  /* USER CODE BEGIN TIM3_Init 0 */

  /* USER CODE END TIM3_Init 0 */

  TIM_ClockConfigTypeDef sClockSourceConfig = {0};
  TIM_MasterConfigTypeDef sMasterConfig = {0};
  TIM_OC_InitTypeDef sConfigOC = {0};

  /* USER CODE BEGIN TIM3_Init 1 */

  /* USER CODE END TIM3_Init 1 */
  htim3.Instance = TIM3;
  htim3.Init.Prescaler = 840-1;
  htim3.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim3.Init.Period = 1000-1;
  htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
  htim3.Init.AutoReloadPreload = TIM_AUTORELOAD_PRELOAD_DISABLE;
  if (HAL_TIM_Base_Init(&htim3) != HAL_OK)
  {
    Error_Handler();
  }
  sClockSourceConfig.ClockSource = TIM_CLOCKSOURCE_INTERNAL;
  if (HAL_TIM_ConfigClockSource(&htim3, &sClockSourceConfig) != HAL_OK)
  {
    Error_Handler();
  }
  if (HAL_TIM_PWM_Init(&htim3) != HAL_OK)
  {
    Error_Handler();
  }
  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  if (HAL_TIMEx_MasterConfigSynchronization(&htim3, &sMasterConfig) != HAL_OK)
  {
    Error_Handler();
  }
  sConfigOC.OCMode = TIM_OCMODE_PWM1;
  sConfigOC.Pulse = 0;
  sConfigOC.OCPolarity = TIM_OCPOLARITY_HIGH;
  sConfigOC.OCFastMode = TIM_OCFAST_DISABLE;
  if (HAL_TIM_PWM_ConfigChannel(&htim3, &sConfigOC, TIM_CHANNEL_1) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN TIM3_Init 2 */

  /* USER CODE END TIM3_Init 2 */
  HAL_TIM_MspPostInit(&htim3);

}

/**
  * @brief USART1 Initialization Function
  * @param None
  * @retval None
  */
static void MX_USART1_UART_Init(void)
{

  /* USER CODE BEGIN USART1_Init 0 */

  /* USER CODE END USART1_Init 0 */

  /* USER CODE BEGIN USART1_Init 1 */

  /* USER CODE END USART1_Init 1 */
  huart1.Instance = USART1;
  huart1.Init.BaudRate = 115200;
  huart1.Init.WordLength = UART_WORDLENGTH_8B;
  huart1.Init.StopBits = UART_STOPBITS_1;
  huart1.Init.Parity = UART_PARITY_NONE;
  huart1.Init.Mode = UART_MODE_TX_RX;
  huart1.Init.HwFlowCtl = UART_HWCONTROL_NONE;
  huart1.Init.OverSampling = UART_OVERSAMPLING_16;
  if (HAL_UART_Init(&huart1) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN USART1_Init 2 */

  /* USER CODE END USART1_Init 2 */

}

/**
  * @brief USART2 Initialization Function
  * @param None
  * @retval None
  */
static void MX_USART2_UART_Init(void)
{

  /* USER CODE BEGIN USART2_Init 0 */

  /* USER CODE END USART2_Init 0 */

  /* USER CODE BEGIN USART2_Init 1 */

  /* USER CODE END USART2_Init 1 */
  huart2.Instance = USART2;
  huart2.Init.BaudRate = 115200;
  huart2.Init.WordLength = UART_WORDLENGTH_8B;
  huart2.Init.StopBits = UART_STOPBITS_1;
  huart2.Init.Parity = UART_PARITY_NONE;
  huart2.Init.Mode = UART_MODE_TX_RX;
  huart2.Init.HwFlowCtl = UART_HWCONTROL_NONE;
  huart2.Init.OverSampling = UART_OVERSAMPLING_16;
  if (HAL_UART_Init(&huart2) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN USART2_Init 2 */

  /* USER CODE END USART2_Init 2 */

}

/**
  * @brief USART6 Initialization Function
  * @param None
  * @retval None
  */
static void MX_USART6_UART_Init(void)
{

  /* USER CODE BEGIN USART6_Init 0 */

  /* USER CODE END USART6_Init 0 */

  /* USER CODE BEGIN USART6_Init 1 */

  /* USER CODE END USART6_Init 1 */
  huart6.Instance = USART6;
  huart6.Init.BaudRate = 115200;
  huart6.Init.WordLength = UART_WORDLENGTH_8B;
  huart6.Init.StopBits = UART_STOPBITS_1;
  huart6.Init.Parity = UART_PARITY_NONE;
  huart6.Init.Mode = UART_MODE_TX_RX;
  huart6.Init.HwFlowCtl = UART_HWCONTROL_NONE;
  huart6.Init.OverSampling = UART_OVERSAMPLING_16;
  if (HAL_UART_Init(&huart6) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN USART6_Init 2 */

  /* USER CODE END USART6_Init 2 */

}

/**
  * @brief GPIO Initialization Function
  * @param None
  * @retval None
  */
static void MX_GPIO_Init(void)
{
  GPIO_InitTypeDef GPIO_InitStruct = {0};

  /* GPIO Ports Clock Enable */
  __HAL_RCC_GPIOC_CLK_ENABLE();
  __HAL_RCC_GPIOH_CLK_ENABLE();
  __HAL_RCC_GPIOA_CLK_ENABLE();
  __HAL_RCC_GPIOB_CLK_ENABLE();

  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(LED_BUILTIN_GPIO_Port, LED_BUILTIN_Pin, GPIO_PIN_RESET);

  /*Configure GPIO pin : LED_BUILTIN_Pin */
  GPIO_InitStruct.Pin = LED_BUILTIN_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(LED_BUILTIN_GPIO_Port, &GPIO_InitStruct);

  /*Configure GPIO pins : CANCEL_Pin OK_Pin UP_Pin DOWN_Pin */
  GPIO_InitStruct.Pin = CANCEL_Pin|OK_Pin|UP_Pin|DOWN_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
  GPIO_InitStruct.Pull = GPIO_PULLUP;
  HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
}

/* USER CODE BEGIN 4 */
// Access Push Button
int PB_OK(){return HAL_GPIO_ReadPin(OK_GPIO_Port, OK_Pin);}
int PB_CANCEL(){return HAL_GPIO_ReadPin(CANCEL_GPIO_Port, CANCEL_Pin);}
int PB_UP(){return HAL_GPIO_ReadPin(UP_GPIO_Port, UP_Pin);}
int PB_DOWN(){return HAL_GPIO_ReadPin(DOWN_GPIO_Port, DOWN_Pin);}
// Access Push Button

// Access LED
void blinkLED(int delay){
	setLED(1); HAL_Delay(delay);
	setLED(0); HAL_Delay(delay);
}

void setLED(int state){
	if (state==1){HAL_GPIO_WritePin(GPIOC, LED_BUILTIN_Pin, 0);}
	else{HAL_GPIO_WritePin(GPIOC, LED_BUILTIN_Pin, 1);}
}
// Access LED

// Access IMU
void getIMUData(void){
	MPU6050_readDMPAll(&pitch, &roll, &yaw);
}
// Access IMU

// Access Servo
long map(long x, long in_min, long in_max, long out_min, long out_max){
	return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}

void setServoTimerPulse(int timerValue){
	htim3.Instance->CCR1 = timerValue;
}

void setServoPosition(int position){
	if (position < 0) position = 0;
	if (position > 180) position = 180;
	htim3.Instance->CCR1 = map(position, 0, 180, 20, 115);
}

void servoSweep(){
	for(int pos = 0; pos<181; pos++){
		setServoPosition(pos);
		HAL_Delay(10);
	}
	for(int pos = 180; pos>0; pos--){
		setServoPosition(pos);
		HAL_Delay(10);
	}
}
// Access Servo


void PIDStabilize(){
	menuShowIMU();

	int pitchSetPoint = 0;
	int servoPos = 90;

	int error = pitch - pitchSetPoint;
	float kp = 5;
	int P = error * kp;
	int servoWrite = servoPos - P;

	setServoPosition(servoWrite);
}

void menuHomeScreen(){
	switch(pilih){
	case 0: pilih = 1; break;

	case 1: SSD1306_GotoXY(0, 0);
			SSD1306_Puts("Home Screen", &Font_7x10, 1);
			SSD1306_GotoXY(0, 10);
			SSD1306_Puts(">Check IMU", &Font_7x10, 1);
			SSD1306_GotoXY(0, 20);
			SSD1306_Puts(" Check Compass", &Font_7x10, 1);
			SSD1306_GotoXY(0, 30);
			SSD1306_Puts(" Check Button", &Font_7x10, 1);
			SSD1306_GotoXY(0, 40);
			SSD1306_Puts(" Run Program", &Font_7x10, 1);
			SSD1306_UpdateScreen();
			if (HAL_GPIO_ReadPin(OK_GPIO_Port, OK_Pin)==0){
				menu = 1;
				HAL_Delay(50);
				SSD1306_Clear();
			}
			break;

	case 2: SSD1306_GotoXY(0, 0);
			SSD1306_Puts("Home Screen", &Font_7x10, 1);
			SSD1306_GotoXY(0, 10);
			SSD1306_Puts(" Check IMU", &Font_7x10, 1);
			SSD1306_GotoXY(0, 20);
			SSD1306_Puts(">Check Compass", &Font_7x10, 1);
			SSD1306_GotoXY(0, 30);
			SSD1306_Puts(" Check Button", &Font_7x10, 1);
			SSD1306_GotoXY(0, 40);
			SSD1306_Puts(" Run Program", &Font_7x10, 1);
			SSD1306_UpdateScreen();
			break;

	case 3: SSD1306_GotoXY(0, 0);
			SSD1306_Puts("Home Screen", &Font_7x10, 1);
			SSD1306_GotoXY(0, 10);
			SSD1306_Puts(" Check IMU", &Font_7x10, 1);
			SSD1306_GotoXY(0, 20);
			SSD1306_Puts(" Check Compass", &Font_7x10, 1);
			SSD1306_GotoXY(0, 30);
			SSD1306_Puts(">Check Button", &Font_7x10, 1);
			SSD1306_GotoXY(0, 40);
			SSD1306_Puts(" Run Program", &Font_7x10, 1);
			SSD1306_UpdateScreen();
			if (HAL_GPIO_ReadPin(OK_GPIO_Port, OK_Pin)==0){
				menu = 3;
				HAL_Delay(50);
				SSD1306_Clear();
			}
			break;

	case 4: SSD1306_GotoXY(0, 0);
			SSD1306_Puts("Home Screen", &Font_7x10, 1);
			SSD1306_GotoXY(0, 10);
			SSD1306_Puts(" Check IMU", &Font_7x10, 1);
			SSD1306_GotoXY(0, 20);
			SSD1306_Puts(" Check Compass", &Font_7x10, 1);
			SSD1306_GotoXY(0, 30);
			SSD1306_Puts(" Check Button", &Font_7x10, 1);
			SSD1306_GotoXY(0, 40);
			SSD1306_Puts(">Run Program", &Font_7x10, 1);
			SSD1306_UpdateScreen();

	case 5: pilih = 4; break;
	}

	if (HAL_GPIO_ReadPin(DOWN_GPIO_Port, DOWN_Pin)==0){
		HAL_Delay(100);
		pilih++;
		SSD1306_UpdateScreen();
	}

	if (HAL_GPIO_ReadPin(UP_GPIO_Port, UP_Pin)==0){
		HAL_Delay(100);
		pilih--;
		SSD1306_UpdateScreen();
	}
}

void menuShowIMU(void){
	getIMUData();

	snprintf(char_roll, sizeof(char_roll), "%.2f", roll);
	snprintf(char_pitch, sizeof(char_roll), "%.2f", pitch);
	snprintf(char_yaw, sizeof(char_roll), "%.2f", yaw);

	SSD1306_GotoXY(0, 0);
	SSD1306_Puts("Checking IMU ", &Font_7x10, 1);

	SSD1306_GotoXY(0, 20);
	SSD1306_Puts("Roll: ", &Font_7x10, 1);
	SSD1306_GotoXY(40, 20);
	SSD1306_Puts(char_roll, &Font_7x10, 1);

	SSD1306_GotoXY(0, 30);
	SSD1306_Puts("Pitch: ", &Font_7x10, 1);
	SSD1306_GotoXY(40, 30);
	SSD1306_Puts(char_pitch, &Font_7x10, 1);

	SSD1306_GotoXY(0, 40);
	SSD1306_Puts("Yaw: ", &Font_7x10, 1);
	SSD1306_GotoXY(40, 40);
	SSD1306_Puts(char_yaw, &Font_7x10, 1);

	SSD1306_UpdateScreen();

	if (HAL_GPIO_ReadPin(CANCEL_GPIO_Port, CANCEL_Pin)==0){
		HAL_Delay(50);
		SSD1306_Clear();
		menu = 0;
	}
}

void menuShowCompass(){

}

void menuShowButton(){
	SSD1306_GotoXY(0, 0);
	SSD1306_Puts("Checking Button ", &Font_7x10, 1);

	if (HAL_GPIO_ReadPin(CANCEL_GPIO_Port, CANCEL_Pin)==0){
		SSD1306_GotoXY(0, 10);
		SSD1306_Puts("Cancel Button", &Font_7x10, 1);

		SSD1306_GotoXY(100, 10);
		SSD1306_Puts("O", &Font_7x10, 1);

		SSD1306_UpdateScreen();
	}

	else if (HAL_GPIO_ReadPin(CANCEL_GPIO_Port, CANCEL_Pin)==1){
		SSD1306_GotoXY(0, 10);
		SSD1306_Puts("Cancel Button", &Font_7x10, 1);

		SSD1306_GotoXY(100, 10);
		SSD1306_Puts("X", &Font_7x10, 1);

		SSD1306_UpdateScreen();
	}

	if (HAL_GPIO_ReadPin(OK_GPIO_Port, OK_Pin)==0){
		SSD1306_GotoXY(0, 20);
		SSD1306_Puts("OK Button", &Font_7x10, 1);

		SSD1306_GotoXY(100, 20);
		SSD1306_Puts("O", &Font_7x10, 1);

		SSD1306_UpdateScreen();
	}

	else if (HAL_GPIO_ReadPin(OK_GPIO_Port, OK_Pin)==1){
		SSD1306_GotoXY(0, 20);
		SSD1306_Puts("OK Button", &Font_7x10, 1);

		SSD1306_GotoXY(100, 20);
		SSD1306_Puts("X", &Font_7x10, 1);

		SSD1306_UpdateScreen();
	}

	if (HAL_GPIO_ReadPin(UP_GPIO_Port, UP_Pin)==0){
		SSD1306_GotoXY(0, 30);
		SSD1306_Puts("Up Button", &Font_7x10, 1);

		SSD1306_GotoXY(100, 30);
		SSD1306_Puts("O", &Font_7x10, 1);

		SSD1306_UpdateScreen();
	}

	else if (HAL_GPIO_ReadPin(UP_GPIO_Port, UP_Pin)==1){
		SSD1306_GotoXY(0, 30);
		SSD1306_Puts("Up Button", &Font_7x10, 1);

		SSD1306_GotoXY(100, 30);
		SSD1306_Puts("X", &Font_7x10, 1);

		SSD1306_UpdateScreen();
	}

	if (HAL_GPIO_ReadPin(DOWN_GPIO_Port, DOWN_Pin)==0){
		SSD1306_GotoXY(0, 40);
		SSD1306_Puts("Down Button", &Font_7x10, 1);

		SSD1306_GotoXY(100, 40);
		SSD1306_Puts("O", &Font_7x10, 1);

		SSD1306_UpdateScreen();
	}

	else if (HAL_GPIO_ReadPin(DOWN_GPIO_Port, DOWN_Pin)==1){
		SSD1306_GotoXY(0, 40);
		SSD1306_Puts("Down Button", &Font_7x10, 1);

		SSD1306_GotoXY(100, 40);
		SSD1306_Puts("X", &Font_7x10, 1);

		SSD1306_UpdateScreen();
	}

	if (HAL_GPIO_ReadPin(CANCEL_GPIO_Port, CANCEL_Pin)==0 && HAL_GPIO_ReadPin(OK_GPIO_Port, OK_Pin)==0){
		HAL_Delay(50);
		SSD1306_Clear();
		menu = 0;
	}
}

void menuRunProgram(){

}

/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  __disable_irq();
  while (1)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */

// These sketches are tested with 2.0.4 ESP32 Arduino Core
// https://github.com/espressif/arduino-esp32/releases/tag/2.0.4

/* Includes ---------------------------------------------------------------- */
#include <XIAOESP32S3_Object_Detection_inferencing.h>
#include "edge-impulse-sdk/dsp/image/image.hpp"
#include "esp_camera.h"

// Select camera model - find more camera models in camera_pins.h file here
// https://github.com/espressif/arduino-esp32/blob/master/libraries/ESP32/examples/Camera/CameraWebServer/camera_pins.h

#define PWDN_GPIO_NUM     -1 
#define RESET_GPIO_NUM    -1 
#define XCLK_GPIO_NUM     10 
#define SIOD_GPIO_NUM     40 
#define SIOC_GPIO_NUM     39
#define Y9_GPIO_NUM       48 
#define Y8_GPIO_NUM       11 
#define Y7_GPIO_NUM       12 
#define Y6_GPIO_NUM       14 
#define Y5_GPIO_NUM       16 
#define Y4_GPIO_NUM       18 
#define Y3_GPIO_NUM       17 
#define Y2_GPIO_NUM       15 
#define VSYNC_GPIO_NUM    38 
#define HREF_GPIO_NUM     47 
#define PCLK_GPIO_NUM     13

/* Constant defines -------------------------------------------------------- */
#define EI_CAMERA_RAW_FRAME_BUFFER_COLS           320
#define EI_CAMERA_RAW_FRAME_BUFFER_ROWS           240
#define EI_CAMERA_FRAME_BYTE_SIZE                 3

/* Private variables ------------------------------------------------------- */
static bool debug_nn = false; // Set this to true to see e.g. features generated from the raw signal
static bool is_initialised = false;
uint8_t *snapshot_buf; //points to the output of the capture

static camera_config_t camera_config = {
    .pin_pwdn = PWDN_GPIO_NUM,
    .pin_reset = RESET_GPIO_NUM,
    .pin_xclk = XCLK_GPIO_NUM,
    .pin_sscb_sda = SIOD_GPIO_NUM,
    .pin_sscb_scl = SIOC_GPIO_NUM,

    .pin_d7 = Y9_GPIO_NUM,
    .pin_d6 = Y8_GPIO_NUM,
    .pin_d5 = Y7_GPIO_NUM,
    .pin_d4 = Y6_GPIO_NUM,
    .pin_d3 = Y5_GPIO_NUM,
    .pin_d2 = Y4_GPIO_NUM,
    .pin_d1 = Y3_GPIO_NUM,
    .pin_d0 = Y2_GPIO_NUM,
    .pin_vsync = VSYNC_GPIO_NUM,
    .pin_href = HREF_GPIO_NUM,
    .pin_pclk = PCLK_GPIO_NUM,

    //XCLK 20MHz or 10MHz for OV2640 double FPS (Experimental)
    .xclk_freq_hz = 20000000,
    .ledc_timer = LEDC_TIMER_0,
    .ledc_channel = LEDC_CHANNEL_0,

    .pixel_format = PIXFORMAT_JPEG, //YUV422,GRAYSCALE,RGB565,JPEG
    .frame_size = FRAMESIZE_QVGA,    //QQVGA-UXGA Do not use sizes above QVGA when not JPEG

    .jpeg_quality = 12, //0-63 lower number means higher quality
    .fb_count = 1,       //if more than one, i2s runs in continuous mode. Use only with JPEG
    .fb_location = CAMERA_FB_IN_PSRAM,
    .grab_mode = CAMERA_GRAB_WHEN_EMPTY,
};

/* Function definitions ------------------------------------------------------- */
bool ei_camera_init(void);
void ei_camera_deinit(void);
bool ei_camera_capture(uint32_t img_width, uint32_t img_height, uint8_t *out_buf) ;

int buzzer = A1;

void setup()
{
    // put your setup code here, to run once:
    Serial.begin(115200);
    pinMode(LED_BUILTIN, OUTPUT); digitalWrite(LED_BUILTIN,HIGH);
    pinMode(buzzer, OUTPUT); 
    playBuzzer(2,50,50);
    digitalWrite(buzzer, HIGH);
    
    //comment out the below line to start inference immediately after upload
    while (!Serial);
    Serial.println("Edge Impulse Inferencing Demo");
    if (ei_camera_init() == false) {
        ei_printf("Failed to initialize Camera!\r\n");
    }
    else {
        ei_printf("Camera initialized\r\n");
    }
    ei_printf("\nStarting continious inference in 2 seconds...\n");
    ei_sleep(2000);
//    playBuzzer(2,50,50);
}

void loop()
{
  int flag = 0;
    // instead of wait_ms, we'll wait on the signal, this allows threads to cancel us...
    if (ei_sleep(5) != EI_IMPULSE_OK) {return;}
    snapshot_buf = (uint8_t*)malloc(EI_CAMERA_RAW_FRAME_BUFFER_COLS * EI_CAMERA_RAW_FRAME_BUFFER_ROWS * EI_CAMERA_FRAME_BYTE_SIZE);

    // check if allocation was successful
    if(snapshot_buf == nullptr) {ei_printf("ERR: Failed to allocate snapshot buffer!\n"); return;}

    ei::signal_t signal;
    signal.total_length = EI_CLASSIFIER_INPUT_WIDTH * EI_CLASSIFIER_INPUT_HEIGHT;
    signal.get_data = &ei_camera_get_data;

    if (ei_camera_capture((size_t)EI_CLASSIFIER_INPUT_WIDTH, (size_t)EI_CLASSIFIER_INPUT_HEIGHT, snapshot_buf) == false) {
        ei_printf("Failed to capture image\r\n");
        free(snapshot_buf);
        return;
    }

    // Run the classifier
    ei_impulse_result_t result = { 0 };
    EI_IMPULSE_ERROR err = run_classifier(&signal, &result, debug_nn);
    if (err != EI_IMPULSE_OK) {
        ei_printf("ERR: Failed to run classifier (%d)\n", err);
        return;
    }

    // print the predictions
    ei_printf("Predictions (DSP: %d ms., Classification: %d ms., Anomaly: %d ms.): \n",
                result.timing.dsp, result.timing.classification, result.timing.anomaly);

#if EI_CLASSIFIER_OBJECT_DETECTION == 1
    ei_printf("Object detection bounding boxes:\r\n");
    for (uint32_t i = 0; i < result.bounding_boxes_count; i++) {
        ei_impulse_result_bounding_box_t bb = result.bounding_boxes[i];
        
        if (bb.value == 0) {
            continue;
        }

        if (bb.label == "Car"){
          flag = 1;
          playBuzzer(3,50,50); 
//          digitalWrite(LED_BUILTIN, HIGH);
          
        }
        
        else if (bb.label == "Motorcycle"){
          flag = 2; 
          playBuzzer(1,100,100);
//          digitalWrite(LED_BUILTIN, HIGH);
        }

        else{
          flag = 0;
        }
        
        ei_printf("  %s (%f) [ x: %u, y: %u, width: %u, height: %u ] state: %d\r\n",
                bb.label,
                bb.value,
                bb.x,
                bb.y,
                bb.width,
                bb.height,
                flag);
    }

    // Print the prediction results (classification)
#else
    ei_printf("Predictions:\r\n");
    for (uint16_t i = 0; i < EI_CLASSIFIER_LABEL_COUNT; i++) {
        ei_printf("  %s: ", ei_classifier_inferencing_categories[i]);
        ei_printf("%.5f\r\n", result.classification[i].value);
    }
#endif

    // Print anomaly result (if it exists)
#if EI_CLASSIFIER_HAS_ANOMALY
    ei_printf("Anomaly prediction: %.3f\r\n", result.anomaly);
#endif

#if EI_CLASSIFIER_HAS_VISUAL_ANOMALY
    ei_printf("Visual anomalies:\r\n");
    for (uint32_t i = 0; i < result.visual_ad_count; i++) {
        ei_impulse_result_bounding_box_t bb = result.visual_ad_grid_cells[i];
        if (bb.value == 0) {
            continue;
        }
        ei_printf("  %s (%f) [ x: %u, y: %u, width: %u, height: %u ]\r\n",
                bb.label,
                bb.value,
                bb.x,
                bb.y,
                bb.width,
                bb.height);
    }
#endif


    free(snapshot_buf);

}

/**
 * @brief   Setup image sensor & start streaming
 *
 * @retval  false if initialisation failed
 */
bool ei_camera_init(void) {

    if (is_initialised) return true;

#if defined(CAMERA_MODEL_ESP_EYE)
  pinMode(13, INPUT_PULLUP);
  pinMode(14, INPUT_PULLUP);
#endif

    //initialize the camera
    esp_err_t err = esp_camera_init(&camera_config);
    if (err != ESP_OK) {
      Serial.printf("Camera init failed with error 0x%x\n", err);
      return false;
    }

    sensor_t * s = esp_camera_sensor_get();
    // initial sensors are flipped vertically and colors are a bit saturated
    if (s->id.PID == OV3660_PID) {
      s->set_vflip(s, 1); // flip it back
      s->set_brightness(s, 1); // up the brightness just a bit
      s->set_saturation(s, 0); // lower the saturation
    }

#if defined(CAMERA_MODEL_M5STACK_WIDE)
    s->set_vflip(s, 1);
    s->set_hmirror(s, 1);
#elif defined(CAMERA_MODEL_ESP_EYE)
    s->set_vflip(s, 1);
    s->set_hmirror(s, 1);
    s->set_awb_gain(s, 1);
#endif

    is_initialised = true;
    return true;
}

/**
 * @brief      Stop streaming of sensor data
 */
void ei_camera_deinit(void) {

    //deinitialize the camera
    esp_err_t err = esp_camera_deinit();

    if (err != ESP_OK)
    {
        ei_printf("Camera deinit failed\n");
        return;
    }

    is_initialised = false;
    return;
}


/**
 * @brief      Capture, rescale and crop image
 *
 * @param[in]  img_width     width of output image
 * @param[in]  img_height    height of output image
 * @param[in]  out_buf       pointer to store output image, NULL may be used
 *                           if ei_camera_frame_buffer is to be used for capture and resize/cropping.
 *
 * @retval     false if not initialised, image captured, rescaled or cropped failed
 *
 */
bool ei_camera_capture(uint32_t img_width, uint32_t img_height, uint8_t *out_buf) {
    bool do_resize = false;

    if (!is_initialised) {
        ei_printf("ERR: Camera is not initialized\r\n");
        return false;
    }

    camera_fb_t *fb = esp_camera_fb_get();

    if (!fb) {
        ei_printf("Camera capture failed\n");
        return false;
    }

   bool converted = fmt2rgb888(fb->buf, fb->len, PIXFORMAT_JPEG, snapshot_buf);

   esp_camera_fb_return(fb);

   if(!converted){
       ei_printf("Conversion failed\n");
       return false;
   }

    if ((img_width != EI_CAMERA_RAW_FRAME_BUFFER_COLS)
        || (img_height != EI_CAMERA_RAW_FRAME_BUFFER_ROWS)) {
        do_resize = true;
    }

    if (do_resize) {
        ei::image::processing::crop_and_interpolate_rgb888(
        out_buf,
        EI_CAMERA_RAW_FRAME_BUFFER_COLS,
        EI_CAMERA_RAW_FRAME_BUFFER_ROWS,
        out_buf,
        img_width,
        img_height);
    }


    return true;
}

static int ei_camera_get_data(size_t offset, size_t length, float *out_ptr)
{
    // we already have a RGB888 buffer, so recalculate offset into pixel index
    size_t pixel_ix = offset * 3;
    size_t pixels_left = length;
    size_t out_ptr_ix = 0;

    while (pixels_left != 0) {
        // Swap BGR to RGB here
        // due to https://github.com/espressif/esp32-camera/issues/379
        out_ptr[out_ptr_ix] = (snapshot_buf[pixel_ix + 2] << 16) + (snapshot_buf[pixel_ix + 1] << 8) + snapshot_buf[pixel_ix];

        // go to the next pixel
        out_ptr_ix++;
        pixel_ix+=3;
        pixels_left--;
    }
    // and done!
    return 0;
}

#if !defined(EI_CLASSIFIER_SENSOR) || EI_CLASSIFIER_SENSOR != EI_CLASSIFIER_SENSOR_CAMERA
#error "Invalid model for current sensor"
#endif

void playBuzzer(int count, int ton, int toff){
  for (int i = 0; i<count;i++){
    digitalWrite(buzzer, HIGH); delay(ton);
    digitalWrite(buzzer, LOW); delay(toff);
  }
}