Published February 28, 2020

Robotic Arm : YOUPI 2020

An automated robotic arm sorting colored objects using computer vision and real-time operating system.

AdvancedFull instructions provided2,829

Story

Introduction

The inspiration for this project arose when I discovered an old robotic arm in our Robotics Club, AeRobotix INSAT. Unfortunately, its condition was suboptimal due to years of neglect and disuse. You can view the original robotic arm at http://youpi.forler.ch/, named the YOUPI Robot.

The primary goal of the project is to construct an automated robotic arm designed for sorting colored objects. This arm will efficiently place items into predefined areas based on their respective colors. The robotic arm will be managed by the real-time operating system FreeRTOS.

In real-world industrial applications, robotic arms serve a variety of functions, including welding, materil handling, thermal spraying, painting, and drilling.

Mechanic DIYs

At the outset of the project, certain components were severely damaged. Consequently, I replaced elements such as the damaged gearing and blocked stepper motors. Additionally, I introduced lubrication liquid to enhance various transmissions and mechanical connections. Unfortunately, the effector's condition was also in a deteriorated state, necessitating its replacement.

1 / 6 • Initial situation of the Arm

Power and Stepper Drivers

To power up this robotic arm, I used an ATX power supply because it is sufficient for this application as it can generate 24V DC for the stepper motors and 12V DC for the vacuum pump. Furthermore, It is small and well-suited to be fixed under the arm.

ATX power supply

Meanwhile, during my research for a suitable stepper driver, I found that the A4988 stepper driver was a good choice due to its compact size and affordable price. Additionally, I utilized a CNC shield V3 to alleviate circuit congestion

1 / 3 • Interfacing A4988 stepper motor driver with microcontroller

ِControl Circuit

In the interest of reducing wiring and making the project more organised, I designed a simple circuit using ISIS Proteus. It contains a relay driving circuit which will supply the vacuum pump with the necessary current and an IR object sensor which will check the presence of the colored objects. Moreover, The stepper motors connections are established directly with CNC shield V3.

Circuit diagram:

Circuit diagram

After printing the design, soldering the components and testing the full functionalities :

1 / 4 • ِControl Circuit

Effector

Unfortunately, the original robot effector suffered significant damage. Consequently, I opted to design my own effector using SolidWorks. The new design uses pneumatic energy, utilizing a vacuum pump and an appropriate ventose.

1 / 4 • The Effector

Image Processing

In this part of the project, I used the computer vision library Open-CV to detect the colored objects and determine their coordinates. This program will run on my computer and will get the data frames from the webcam ( The camera is fixed on the top of the arm ). Then, it will analyse those frames and check for objects within the defined radius. The order of priority is (First to Last):

When an object is detected, it simply sends its coordinates and color to the STM32 Micro-controller. Then, the main program of the arm will convert these coordinates to the robot's real coordinate system.

You can find this program Object_Sorting.py in the Code section below or in my GitHub repository: https://github.com/AlaHammouda/Robotic-Arm-Project.

I used the range-detector script in the imutils library to fix the lower and upper boundaries in the HSV color space.

Test Demonstration:

Main program :

At the apex of the pyramid, I programmed the arm utilizing the STM32F410RB NUCLEO Board. Recognizing the need to control the stepper motors simultaneously, I discovered that a real-time operating system (RTOS) not only enhances efficiency but also enables the breakdown of a complex problem into simpler components. This approach allows one to concentrate on the development of each task rather than managing scheduling intricacies. FreeRTOS, a free real-time embedded operating system, emerged as the optimal solution. It is professionally developed, rigorously quality-controlled, robust, and well-supported. Moreover, it is compatible with various popular microcontrollers, including the STM32.

Theoretical study :

To determine the specific joint angles (θ1, θ2, θ3, θ4) for any (x, y, z) coordinates of the effector, as it is basically an essential operation in the program, I drew a simplified scheme:

The resolution of this system of equations is so complex and will lead us to an infinite number of solutions. Therefore, I decided to simplify them by fixing the angle θ4= -π/2 rad :

Now, the resolution of the system will give us two solutions. I chose the one in which (θ2>θ3) because it is more adapted and convenient for the arm :

This solution is only determined and tested with :

Code Implementation :

After configuring the microcontroller settings in STM32CubeMX and generating the setup code in Keil uVision IDE, the only task left is to implement the code.

The program's general idea is very simple, It's just about running four tasks in FreeRTOS: Three of them are managing the stepper motors and the fourth one is handling specific operations in every state of the arm.

In order to ensure real time interaction with the objects, the communication with my computer is established using UART interrupts.

Including libraries and declaring variables : main.c(56)

Definition and creation of tasks : main.c(208)

Start the RTOS scheduler: main.c(233)

Implementing Set_Joint_Angles( ) : This function will convert (x, y, z) command coordinates to (θ1, θ2, θ3, θ4) joint angles as we have already explained in the previous section : main.c(485)

Implementing Stepper1_Task_function( ) : this function will handle the rotation of Joint1. The same code is in the Stepper2_Task_function( ) and Stepper3_Task_function( ) main.c(516)

Implementing Main_Arm_Task_function( ): This function will handle the state of the arm and the appropriate action in each situation: main.c(600)

Declaring variables in stm32f4xx_it.c(41) :

Receiving and handling data of UART interrupts : stm32f4xx_it.c(104)

You can find those files ( main.c, stm32f4xx_it.c ) in the code section below. The full program project (Keil µVision + STM32cubeMX files ) is available in my GitHub repository : https://github.com/AlaHammouda/Robotic-Arm-Project.

Testing the Main Program:

Finally

This project has been a remarkable experience preceding the completion of my engineering studies. I applied a significant amount of knowledge acquired during my academic studies and extracurricular activities. Additionally, I acquired new skills, particularly in computer vision and embedded systems programming.

Finally, I would like to express my gratitude and appreciation to my wonderful family at AeRobotix INSAT for the invaluable support and the wealth of knowledge I gained throughout the course of this project.

If you need more information, don't hesitate to contact me : hammouda.alaaeddine@gmail.com

1 / 7

Schematics

Code

/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * This notice applies to any and all portions of this file
  * that are not between comment pairs USER CODE BEGIN and
  * USER CODE END. Other portions of this file, whether 
  * inserted by the user or by software development tools
  * are owned by their respective copyright owners.
  *
  * Copyright (c) 2020 STMicroelectronics International N.V. 
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted, provided that the following conditions are met:
  *
  * 1. Redistribution 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 STMicroelectronics nor the names of other 
  *    contributors to this software may be used to endorse or promote products 
  *    derived from this software without specific written permission.
  * 4. This software, including modifications and/or derivative works of this 
  *    software, must execute solely and exclusively on microcontroller or
  *    microprocessor devices manufactured by or for STMicroelectronics.
  * 5. Redistribution and use of this software other than as permitted under 
  *    this license is void and will automatically terminate your rights under 
  *    this license. 
  *
  * THIS SOFTWARE IS PROVIDED BY STMICROELECTRONICS AND CONTRIBUTORS "AS IS" 
  * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
  * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
  * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT 
  * SHALL STMICROELECTRONICS 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.
  *
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "stm32f4xx_hal.h"
#include "cmsis_os.h"

/* USER CODE BEGIN Includes */
	
#include "math.h"
#include "string.h"
#include <stdlib.h>
#include <stdio.h>	

#define PI                3.141592653
#define JOINT1_RESOLUTION 0.060944641 /*  Joint1 resolution in degree  */
#define JOINT2_RESOLUTION 0.055333713 /*  Joint2 resolution in degree  */
#define JOINT3_RESOLUTION 0.055301333 /*  Joint3 resolution in degree  */   
#define X_INIT            132.86198   /*  x value of the initial position */
#define Y_INIT            0.00        /*  y value of the initial position */
#define Z_INIT            -25.3336658 /*  z value of the initial position */
#define SENS_SWITCH_DELAY 100         /*  Short delay, When switching the sens of the stepper */
#define MAX_ARM_RANGE     324         /*  The maximum range that the effector can reach */
#define l2                162.00      /*  Length of the arm 2  */
#define l3                162.00      /*  Length of the arm 3  */
#define l4                122.5       /*  Length of the arm 4  */
	
	
typedef struct{                                    
	short x;                           
	short y;
	short z;
	}Color_Area;                          
	
	
/* USER CODE END Includes */

/* Private variables ---------------------------------------------------------*/
ADC_HandleTypeDef hadc1;

TIM_HandleTypeDef htim6;

UART_HandleTypeDef huart2;

osThreadId defaultTaskHandle;
osThreadId Stepper1_TaskHandle;
osThreadId Stepper2_TaskHandle;
osThreadId Stepper3_TaskHandle;
osThreadId Stepper4_TaskHandle;
osThreadId Main_Arm_TaskHandle;

/* USER CODE BEGIN PV */
/* Private variables ---------------------------------------------------------*/
 	
const char SLEEPING='z';   
const char TRACKING ='t';
const char SORTING='s';
			
float O1=0;        /* Current O1 joint in degree */ 
float O2=-35.2;    /* Current O2 joint in degree */ 
float O3=60.1;     /* Current O3 joint in degree */
float O4=-90;      /* Current O4 joint in degree */
float O1_t=0;      /* O1 of the current target  */
float O2_t=90;     /* O2 of the current target  */
float O3_t=-35;    /* O3 of the current target  */
float O4_t=-90;    /* O1 of the current target  */

char state = SLEEPING;    /* Status of the Arm  */
unsigned char Step1_done,Step2_done,Step3_done = 0;  /* Status of each Stepper Motor  */
short sens_1=1;     /* Current sens of the stepper 1 */
short sens_2=1;     /* Current sens of the stepper 2 */
short sens_3=-1;    /* Current sens of the stepper 3 */
short  x_target=0;  /* x value of the current target */
short  y_target=0;  /* y value of the current target */

char	PC_Data[7]={0};												        		    
char	x_tab[3]={0};      
char	y_tab[3]={0};

char color ='g';                        /* Color of the current object */                      
Color_Area  Green_Area={150,195,-130};  /* Coordinates of the green area */
Color_Area  Red_Area={250,195,-130};    /* Coordinates of the red area */
Color_Area  Blue_Area={150,-195,-130};  /* Coordinates of the blue area */
Color_Area  Yellow_Area={250,-195,-130};/* Coordinates of the yellow area */

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_USART2_UART_Init(void);
static void MX_TIM6_Init(void);
static void MX_ADC1_Init(void);
void Stepper1_Task_function(void const * argument);
void Stepper2_Task_function(void const * argument);
void Stepper3_Task_function(void const * argument);
void Main_Arm_Task_function(void const * argument);

/* USER CODE BEGIN PFP */
/* Private function prototypes -----------------------------------------------*/

int Check_Object(void);             /*  Verify if there is an object or no  */
unsigned char Steppers_Ready(void);	/*  Get the state of all Stepper Motors  */ 
/*  Set(O1_t,O2_t,O3_t,O4_t) of the new target */
void Set_Joint_Angles(float xt,float yt,float zt);     

/* USER CODE END PFP */

/* USER CODE BEGIN 0 */

/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  *
  * @retval None
  */
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_USART2_UART_Init();
  MX_TIM6_Init();
  MX_ADC1_Init();
  /* USER CODE BEGIN 2 */
 	__HAL_UART_ENABLE_IT(&huart2,UART_IT_RXNE);
  /* USER CODE END 2 */

  /* USER CODE BEGIN RTOS_MUTEX */
  /* add mutexes, ... */
  /* USER CODE END RTOS_MUTEX */

  /* USER CODE BEGIN RTOS_SEMAPHORES */
  /* add semaphores, ... */
  /* USER CODE END RTOS_SEMAPHORES */

  /* USER CODE BEGIN RTOS_TIMERS */
  /* start timers, add new ones, ... */
  /* USER CODE END RTOS_TIMERS */

  /* Create the thread(s) */

  /* definition and creation of Stepper1_Task */
  osThreadDef(Stepper1_Task, Stepper1_Task_function, osPriorityNormal, 0, 128);
  Stepper1_TaskHandle = osThreadCreate(osThread(Stepper1_Task), NULL);

  /* definition and creation of Stepper2_Task */
  osThreadDef(Stepper2_Task, Stepper2_Task_function,osPriorityNormal, 0, 128);
  Stepper2_TaskHandle = osThreadCreate(osThread(Stepper2_Task), NULL);

  /* definition and creation of Stepper3_Task */
  osThreadDef(Stepper3_Task, Stepper3_Task_function, osPriorityNormal, 0, 128);
  Stepper3_TaskHandle = osThreadCreate(osThread(Stepper3_Task), NULL);


  /* definition and creation of Main_Arm_Task */
  osThreadDef(Main_Arm_Task, Main_Arm_Task_function, osPriorityNormal, 0, 128);
  Main_Arm_TaskHandle = osThreadCreate(osThread(Main_Arm_Task), NULL);

  /* USER CODE BEGIN RTOS_THREADS */
  /* add threads, ... */
  /* USER CODE END RTOS_THREADS */

  /* USER CODE BEGIN RTOS_QUEUES */
  /* add queues, ... */
  /* USER CODE END RTOS_QUEUES */

  /* Start scheduler */
 osKernelStart();
  
  /* We should never get here as control is now taken by the scheduler */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  while (1)
  {

  /* USER CODE END WHILE */

  /* USER CODE BEGIN 3 */
		
  }
  /* USER CODE END 3 */

}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{

  RCC_OscInitTypeDef RCC_OscInitStruct;
  RCC_ClkInitTypeDef RCC_ClkInitStruct;

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

  __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE1);

    /**Initializes the CPU, AHB and APB busses clocks 
    */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSI;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.HSICalibrationValue = 16;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSI;
  RCC_OscInitStruct.PLL.PLLM = 8;
  RCC_OscInitStruct.PLL.PLLN = 100;
  RCC_OscInitStruct.PLL.PLLP = RCC_PLLP_DIV2;
  RCC_OscInitStruct.PLL.PLLQ = 4;
  RCC_OscInitStruct.PLL.PLLR = 2;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

    /**Initializes the CPU, AHB and APB busses 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_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_3) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

    /**Configure the Systick interrupt time 
    */
  HAL_SYSTICK_Config(HAL_RCC_GetHCLKFreq()/1000);

    /**Configure the Systick 
    */
  HAL_SYSTICK_CLKSourceConfig(SYSTICK_CLKSOURCE_HCLK);

  /* SysTick_IRQn interrupt configuration */
  HAL_NVIC_SetPriority(SysTick_IRQn, 15, 0);
}

/* ADC1 init function */
static void MX_ADC1_Init(void)
{

  ADC_ChannelConfTypeDef sConfig;

    /**Configure the global features of the ADC (Clock, Resolution, Data Alignment and number of conversion) 
    */
  hadc1.Instance = ADC1;
  hadc1.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4;
  hadc1.Init.Resolution = ADC_RESOLUTION_10B;
  hadc1.Init.ScanConvMode = DISABLE;
  hadc1.Init.ContinuousConvMode = DISABLE;
  hadc1.Init.DiscontinuousConvMode = DISABLE;
  hadc1.Init.ExternalTrigConvEdge = ADC_EXTERNALTRIGCONVEDGE_NONE;
  hadc1.Init.ExternalTrigConv = ADC_SOFTWARE_START;
  hadc1.Init.DataAlign = ADC_DATAALIGN_RIGHT;
  hadc1.Init.NbrOfConversion = 1;
  hadc1.Init.DMAContinuousRequests = DISABLE;
  hadc1.Init.EOCSelection = ADC_EOC_SINGLE_CONV;
  if (HAL_ADC_Init(&hadc1) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

    /**Configure for the selected ADC regular channel its corresponding rank in the sequencer and its sample time. 
    */
  sConfig.Channel = ADC_CHANNEL_0;
  sConfig.Rank = 1;
  sConfig.SamplingTime = ADC_SAMPLETIME_3CYCLES;
  if (HAL_ADC_ConfigChannel(&hadc1, &sConfig) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

}

/* TIM6 init function */
static void MX_TIM6_Init(void)
{

  TIM_MasterConfigTypeDef sMasterConfig;

  htim6.Instance = TIM6;
  htim6.Init.Prescaler = 2;
  htim6.Init.CounterMode = TIM_COUNTERMODE_UP;
  htim6.Init.Period = 2;
  if (HAL_TIM_Base_Init(&htim6) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

  sMasterConfig.MasterOutputTrigger = TIM_TRGO_RESET;
  sMasterConfig.MasterSlaveMode = TIM_MASTERSLAVEMODE_DISABLE;
  if (HAL_TIMEx_MasterConfigSynchronization(&htim6, &sMasterConfig) != HAL_OK)
  {
    _Error_Handler(__FILE__, __LINE__);
  }

}

/* USART2 init function */
static void MX_USART2_UART_Init(void)
{

  huart2.Instance = USART2;
  huart2.Init.BaudRate = 9600;
  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(__FILE__, __LINE__);
  }

}

/** Configure pins as 
        * Analog 
        * Input 
        * Output
        * EVENT_OUT
        * EXTI
*/
static void MX_GPIO_Init(void)
{

  GPIO_InitTypeDef GPIO_InitStruct;

  /* 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(GPIOA, LD2_Pin|GPIO_PIN_6|GPIO_PIN_8|GPIO_PIN_9 
                          |GPIO_PIN_10, GPIO_PIN_RESET);

  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(GPIOC, GPIO_PIN_4|GPIO_PIN_9|GPIO_PIN_10, GPIO_PIN_RESET);

  /*Configure GPIO pin Output Level */
  HAL_GPIO_WritePin(GPIOB, GPIO_PIN_10|GPIO_PIN_3|GPIO_PIN_4|GPIO_PIN_5, GPIO_PIN_RESET);

  /*Configure GPIO pin : B1_Pin */
  GPIO_InitStruct.Pin = B1_Pin;
  GPIO_InitStruct.Mode = GPIO_MODE_IT_FALLING;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  HAL_GPIO_Init(B1_GPIO_Port, &GPIO_InitStruct);

  /*Configure GPIO pins : LD2_Pin PA8 PA9 */
  GPIO_InitStruct.Pin = LD2_Pin|GPIO_PIN_8|GPIO_PIN_9;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);

  /*Configure GPIO pins : PA6 PA10 */
  GPIO_InitStruct.Pin = GPIO_PIN_6|GPIO_PIN_10;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_PULLUP;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);

  /*Configure GPIO pins : PC4 PC9 PC10 */
  GPIO_InitStruct.Pin = GPIO_PIN_4|GPIO_PIN_9|GPIO_PIN_10;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);

  /*Configure GPIO pins : PB10 PB4 */
  GPIO_InitStruct.Pin = GPIO_PIN_10|GPIO_PIN_4;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_NOPULL;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);

  /*Configure GPIO pins : PB3 PB5 */
  GPIO_InitStruct.Pin = GPIO_PIN_3|GPIO_PIN_5;
  GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
  GPIO_InitStruct.Pull = GPIO_PULLUP;
  GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
  HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);

}

/* USER CODE BEGIN 4 */

int sign(float x){
	return (x > 0) - (x < 0);
}
	
   /* Convert radians to degrees */
float rad_to_deg(double rad){                       
    return(rad*360/(2*PI));
}

   /* 1 <=> Object detected    0 <=> No Object detected  */
int Check_Object(){	
  int  adc=0;
  for(int i=0;i<100;i++){		
     HAL_ADC_Start(&hadc1);
     adc+=HAL_ADC_GetValue(&hadc1);	
  }
  adc/=100;
  return (adc<100);
}

		/*  Set(O1_t,O2_t,O3_t,O4_t) of the new target */
void  Set_Joint_Angles(float xt,float yt , float zt){		
	 	
  float a=sqrt(pow(xt,2)+pow(yt,2));
  float sphere_radius=sqrt(a*a+pow((zt+l4),2));
  float alpha=(a*a+pow((zt+l4),2)+pow(l3,2)-pow(l2,2))/(2*l3);                                                           
  float beta=atan((zt+l4)/a);
  /* Verifying that the new target is in the range of the arm  */
  if(sphere_radius<=MAX_ARM_RANGE){ 
	
    if(xt==0) O1_t=PI/2*sign(yt); else  O1_t=atan(yt/xt);   
     O2_t=acos(alpha/sphere_radius)+beta;
     O3_t=acos((a-l3*cos(O2_t))/l2)*sign(zt);
			
     O1_t=rad_to_deg(O1_t); /* Converting angles from radians to degrees  */ 
     O2_t=rad_to_deg(O2_t);
     O3_t=rad_to_deg(O3_t);
     O4_t=-90.00;
     /* Initializing steppers state to 0  */
     Step1_done=0; Step2_done=0; Step3_done=0; 
	}	
}

		 /*  Get the state of all Stepper Motors  */ 
unsigned char Steppers_Ready(){
	return(Step1_done && Step2_done && Step3_done); 
}

/* USER CODE END 4 */


/* Stepper1_Task_function function */
void Stepper1_Task_function(void const * argument)
{
  /* USER CODE BEGIN Stepper1_Task_function */
  /* Infinite loop */
  for(;;)
  {	
    if(fabs(O1_t-O1)>(JOINT1_RESOLUTION+0.01)){		                      
       if((O1<O1_t)&&(sens_1==-1)){osDelay(SENS_SWITCH_DELAY); sens_1= 1; }  /*  Short delay while reversing    */
       if((O1>O1_t)&&(sens_1==1)){osDelay(SENS_SWITCH_DELAY);  sens_1= -1;}	/*	the sens of the stepper motor */
				
        HAL_GPIO_WritePin(GPIOB,GPIO_PIN_4,sens_1-1); /*  Setting the direction of the stepper */  
			
        HAL_GPIO_WritePin(GPIOA,GPIO_PIN_10,1); /* Sending one pulse command to the stepper motor <=> 1 step */
        osDelay(1);                                  
        HAL_GPIO_WritePin(GPIOA,GPIO_PIN_10,0);
        osDelay(1);
	  
        O1+=(JOINT1_RESOLUTION*sens_1); /* Incrementing or Decrementing the joint angle with each step */
     }
     else{
       Step1_done=1; /* Setting the stepper state to 1 */
       osDelay(1);
     }			
   }
  /* USER CODE END Stepper1_Task_function */
}

/* Stepper2_Task_function function */
void Stepper2_Task_function(void const * argument)
{
  /* USER CODE BEGIN Stepper2_Task_function */
  /* Infinite loop */
  for(;;)
  {
     if(fabs(O2_t-O2)>(JOINT2_RESOLUTION+0.01)){		
        if((O2>O2_t)&&(sens_2==1)){	 osDelay(SENS_SWITCH_DELAY); sens_2= -1;} /*  Short delay while reversing  */
        if((O2<O2_t)&&(sens_2==-1)){ osDelay(SENS_SWITCH_DELAY);  sens_2= 1;}	/*	the sens of the stepper motor */

        HAL_GPIO_WritePin(GPIOA,GPIO_PIN_8,sens_2-1);/*  Setting the direction of the stepper */  
					
        HAL_GPIO_WritePin(GPIOB,GPIO_PIN_5,1); /* Sending one pulse command to the stepper motor <=> 1 step */
        osDelay(1);    
        HAL_GPIO_WritePin(GPIOB,GPIO_PIN_5,0);
        osDelay(1);
					
        O2+=(JOINT2_RESOLUTION*sens_2); /* Incrementing or Decrementing the joint angle with each step */
     }
     else{
       Step2_done=1;	/* Setting the stepper state to 1 */
       osDelay(1);
     }
   }
  /* USER CODE END Stepper2_Task_function */
}

/* Stepper3_Task_function function */
void Stepper3_Task_function(void const * argument)
{
  /* USER CODE BEGIN Stepper3_Task_function */
  /* Infinite loop */
  for(;;)
  {
       if(fabs(O3_t-O3)>(JOINT3_RESOLUTION+0.01)){		
          if((O3<O3_t)&&(sens_3==-1)){osDelay(SENS_SWITCH_DELAY); sens_3= 1;} /*  Short delay while reversing  */
          if((O3>O3_t)&&(sens_3==1)){osDelay(SENS_SWITCH_DELAY);  sens_3=-1;}	/*	the sens of the stepper motor */
					
          HAL_GPIO_WritePin(GPIOA,GPIO_PIN_5,sens_3+1); /*  Setting the direction of the stepper */  

          HAL_GPIO_WritePin(GPIOA,GPIO_PIN_6,1); /* Sending one pulse command to the stepper motor <=> 1 step */
          osDelay(1);   
          HAL_GPIO_WritePin(GPIOA,GPIO_PIN_6,0);
          osDelay(1);  
					
          O3+=(JOINT3_RESOLUTION*sens_3); /* Incrementing or Decrementing the joint angle with each step */	 
       }
       else{
          Step3_done=1;	/* Setting the stepper state to 1 */
          osDelay(1);
        }
    }
  /* USER CODE END Stepper3_Task_function */
}

/* Main_Arm_Task_function function */
void Main_Arm_Task_function(void const * argument)
{
  /* Infinite loop */
  for(;;)
  {
     if(Steppers_Ready() && state==TRACKING ){		
        if(Check_Object()){  /* Object detected */
				 
          state=SORTING;
          HAL_GPIO_WritePin(GPIOC,GPIO_PIN_4,1);  /* pump ON */  				 
          Set_Joint_Angles(x_target,y_target,-178);  /* Get object */ 
          while(!Steppers_Ready());		 
          Set_Joint_Angles(x_target,y_target,-116);	 /* Pick object up  */
          while(!Steppers_Ready()); 	     			 
          switch (color){   /* Place each object in the appropriate color area  */
            case 'g':Set_Joint_Angles(Green_Area.x,Green_Area.y,Green_Area.z);    Green_Area.z+=25;   break;    
            case 'r':Set_Joint_Angles(Red_Area.x,Red_Area.y,Red_Area.z);          Red_Area.z+=25;     break;
            case 'b':Set_Joint_Angles(Blue_Area.x,Blue_Area.y,Blue_Area.z);       Blue_Area.z+=25;    break;
            case 'y':Set_Joint_Angles(Yellow_Area.x,Yellow_Area.y,Yellow_Area.z); Yellow_Area.z+=25;  break;
            default : break;
          }     			 
       }
        else{  /* No object detected */
          state=SLEEPING;  
          Set_Joint_Angles(X_INIT,Y_INIT,Z_INIT); /* Set the Arm to the initial position */	
        }
      }
    if(Steppers_Ready() && state==SORTING ){
        HAL_GPIO_WritePin(GPIOC,GPIO_PIN_4,0); /* pump OFF */  
        while(Check_Object());                      
        Set_Joint_Angles(X_INIT,Y_INIT,Z_INIT); /* Set the Arm to the initial position */	
        state=SLEEPING;
     }
    osDelay(1);
  }
  /* USER CODE END Main_Arm_Task_function */
}

/**
  * @brief  Period elapsed callback in non blocking mode
  * @note   This function is called  when TIM1 interrupt took place, inside
  * HAL_TIM_IRQHandler(). It makes a direct call to HAL_IncTick() to increment
  * a global variable "uwTick" used as application time base.
  * @param  htim : TIM handle
  * @retval None
  */
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
  /* USER CODE BEGIN Callback 0 */

  /* USER CODE END Callback 0 */
  if (htim->Instance == TIM1) {
    HAL_IncTick();
  }
  /* USER CODE BEGIN Callback 1 */

  /* USER CODE END Callback 1 */
}

/**
  * @brief  This function is executed in case of error occurrence.
  * @param  file: The file name as string.
  * @param  line: The line in file as a number.
  * @retval None
  */
void _Error_Handler(char *file, int line)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  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,
     tex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */

/**
  * @}
  */

/**
  * @}
  */

/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/

/**
  ******************************************************************************
  * @file    stm32f4xx_it.c
  * @brief   Interrupt Service Routines.
  ******************************************************************************
  *
  * COPYRIGHT(c) 2020 STMicroelectronics
  *
  * 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 STMicroelectronics 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.
  *
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include "stm32f4xx_hal.h"
#include "stm32f4xx.h"
#include "stm32f4xx_it.h"
#include "cmsis_os.h"

/* USER CODE BEGIN 0 */

extern	short x_target;                         
extern	short y_target;
extern	char	PC_Data[7];  
extern	char	x_tab[3];        
extern	char	y_tab[3];
unsigned char char_received ;              /* Count of the received data */
extern  char  state;
extern  const char  SORTING;	
extern  const char  SLEEPING;  
extern  const char  TRACKING;
extern  void Set_Joint_Angles(float xt,float yt,float zt);
extern char color;

/* USER CODE END 0 */

/* External variables --------------------------------------------------------*/
extern TIM_HandleTypeDef htim6;
extern UART_HandleTypeDef huart2;

extern TIM_HandleTypeDef htim1;

/******************************************************************************/
/*            Cortex-M4 Processor Interruption and Exception Handlers         */ 
/******************************************************************************/

/**
* @brief This function handles System tick timer.
*/
void SysTick_Handler(void)
{
  /* USER CODE BEGIN SysTick_IRQn 0 */

  /* USER CODE END SysTick_IRQn 0 */
  osSystickHandler();
  /* USER CODE BEGIN SysTick_IRQn 1 */

  /* USER CODE END SysTick_IRQn 1 */
}

/******************************************************************************/
/* STM32F4xx Peripheral Interrupt Handlers                                    */
/* Add here the Interrupt Handlers for the used peripherals.                  */
/* For the available peripheral interrupt handler names,                      */
/* please refer to the startup file (startup_stm32f4xx.s).                    */
/******************************************************************************/

/**
* @brief This function handles TIM1 update interrupt.
*/
void TIM1_UP_IRQHandler(void)
{
  /* USER CODE BEGIN TIM1_UP_IRQn 0 */

  /* USER CODE END TIM1_UP_IRQn 0 */
  HAL_TIM_IRQHandler(&htim1);
  /* USER CODE BEGIN TIM1_UP_IRQn 1 */

  /* USER CODE END TIM1_UP_IRQn 1 */
}

/**
* @brief This function handles USART2 global interrupt.
*/
void USART2_IRQHandler(void)
{
  /* USER CODE BEGIN USART2_IRQn 0 */

  /* USER CODE END USART2_IRQn 0 */
  HAL_UART_IRQHandler(&huart2);
  /* USER CODE BEGIN USART2_IRQn 1 */
	
	HAL_UART_Receive_IT(&huart2,(uint8_t *)&PC_Data,7);     /* Receive PC_data [ x , y , color  ]  */
	char_received++;							
	
	if(char_received==7){
		x_tab[0] = PC_Data[0];     
		x_tab[1] = PC_Data[1];  
		x_tab[2] = PC_Data[2];    
		y_tab[0] = PC_Data[3];     
		y_tab[1] = PC_Data[4];   
		y_tab[2] = PC_Data[5];	
		color    = PC_Data[6];	
							
		if(state==SLEEPING){    /* ONLY proceed if the arm is in a SLEEPING state */
		  y_target=atoi(y_tab);   /* convert y_tab to an integer value */     
		  x_target=atoi(x_tab);   /* convert x_tab to an  integer value */  
		  y_target=0.4972*y_target-155.623;  /* convert y_target to the arm coordinate system  */  
		  x_target=-0.506*x_target+327.53;   /* convert x_target to the arm coordinate system  */  								
		  Set_Joint_Angles(x_target,y_target,-146);  /*  Start Tracking the colored object  */ 
		  state=TRACKING;				
		 }
	char_received=0;
	 }
  /* USER CODE END USART2_IRQn 1 */
  }

/**
* @brief This function handles TIM6 global interrupt and DAC channel underrun error interrupt.
*/
void TIM6_DAC_IRQHandler(void)
{
  /* USER CODE BEGIN TIM6_DAC_IRQn 0 */

  /* USER CODE END TIM6_DAC_IRQn 0 */
  HAL_TIM_IRQHandler(&htim6);
  /* USER CODE BEGIN TIM6_DAC_IRQn 1 */

  /* USER CODE END TIM6_DAC_IRQn 1 */
}

/* USER CODE BEGIN 1 */

/* USER CODE END 1 */
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/

# import the necessary packages.
from imutils.video import VideoStream
import numpy as np
import cv2
import imutils
import time
import serial

# configure the serial connections
ser = serial.Serial('COM12',baudrate=9600, timeout=1)

# define the lower and upper boundaries of the "green"
greenLower = (45,44,100)  
greenUpper = (85, 255, 255)

# define the lower and upper boundaries of the "red"
redLower   = (0, 50, 145)
redUpper   = (7, 255, 255)

# define the lower and upper boundaries of the "blue"
blueLower  = (95, 170, 123)
blueUpper  = (111, 255, 255)

# define the lower and upper boundaries of the "yellow"
yellowLower= (21, 90, 100)
yellowUpper= (35, 255, 255)


def Grab_Contours(hsv_space,colorLower,colorUpper):
	# construct a mask for the color , then perform
	# a series of dilations and erosions to remove any small
	# blobs left in the mask
	mask = cv2.inRange(hsv_space, colorLower, colorUpper)
	mask = cv2.erode(mask, None, iterations=2)
	mask = cv2.dilate(mask, None, iterations=2)
	# find contours in the mask	
	cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE)
	cnts = imutils.grab_contours(cnts)
	return cnts

def Process_Color(cnts,color,color_key):
	c = max(cnts, key=cv2.contourArea)
	((x, y), radius) = cv2.minEnclosingCircle(c)
	M = cv2.moments(c)
	center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
        # only proceed if the radius is within 
	if radius > 70 and radius < 75:
                # draw the circle
		cv2.circle(frame, (int(x), int(y)), int(radius),color, 2)
		STM_Data = (str(center[1]).zfill(3))+(str(center[0]).zfill(3)+color_key)
		ser.write(str.encode(STM_Data))
		return 1
	else:
		return 0

object_detected=0

# grab the reference to the webcam
# allow the camera to warm up
vs = VideoStream(src=1).start()
time.sleep(2.0)

# keep looping
while True:
	# grab the current frame
	frame = vs.read()

	# resize the frame, blur it, and convert it to the HSV color space
	frame = imutils.resize(frame, width=600)
	blurred = cv2.GaussianBlur(frame, (11, 11), 0)
	hsv = cv2.cvtColor(blurred, cv2.COLOR_BGR2HSV)    

        # get contours for each color  
	cnts_g = Grab_Contours(hsv,greenLower,greenUpper)
	cnts_r = Grab_Contours(hsv,redLower,redUpper)
	cnts_b = Grab_Contours(hsv,blueLower,blueUpper)
	cnts_y = Grab_Contours(hsv,yellowLower,yellowUpper)

	object_detected = 0

	# only proceed if at least one contour was found for green 
	if len(cnts_g) > 0:
			object_detected = Process_Color(cnts_g,(0,255,0),'g')
	# only proceed if at least one contour was found for red 		
	if len(cnts_r) > 0 and not(object_detected):
			object_detected = Process_Color(cnts_r,(0,0,255),'r')
	# only proceed if at least one contour was found for blue 		
	if len(cnts_b) > 0 and not(object_detected):
			object_detected = Process_Color(cnts_b,(255,0,0),'b')
        # only proceed if at least one contour was found for yellow 
	if len(cnts_y) > 0 and not(object_detected):
			object_detected = Process_Color(cnts_y,(0,255,255),'y')

        # show the frame to our screen
	cv2.imshow("Frame", frame)
	key = cv2.waitKey(1) & 0xFF
 
	if key == ord("q"):
		break

	# process data every 150 ms 
	time.sleep(0.15)

# release the camera
# close all windows
vs.release()
cv2.destroyAllWindows()

Credits

HAMMOUDA Ala Eddine

1 project • 6 followers

Contact

Thanks to AeRobotix INSAT .

Comments

Please log in or sign up to comment.

Robotic Arm : YOUPI 2020

Story

Introduction

Mechanic DIYs

Power and Stepper Drivers

ِControl Circuit

Effector

Image Processing

Main program :

Finally

Custom parts and enclosures

Effector support

YOUPI 2020

Schematics

Circuit diagram

Code

main.c

stm32f4xx_it.c

Object_Sorting.py

Robotic-Arm-Project

Credits

HAMMOUDA Ala Eddine

Comments

Embed the widget on your own site

Robotic Arm : YOUPI 2020

Robotic Arm : YOUPI 2020

Story

Introduction

Mechanic DIYs

Power and Stepper Drivers

ِControl Circuit

Effector

Image Processing

Main program :

Finally

Custom parts and enclosures

Effector support

YOUPI 2020

Schematics

Circuit diagram

Code

main.c

stm32f4xx_it.c

Object_Sorting.py

Robotic-Arm-Project

Credits

HAMMOUDA Ala Eddine

Comments

Related channels and tags