Close

10. Sorting Objects With A 3D Magnetic Sensor - Part 2

A project log for Work-holding Robotic Arm for Electronic Workshop

Collaborative Robotic Arm Used as Assistant in an Electronic Workshop And Controlled by Voice Commands and Capsense Technology

Below you have the code uploaded to the 3D Magnetic Sensor 2Go board: magnetic_sensor.ino

// AUTHOR: GUILLERMO PEREZ GUILLEN


#include <Tle493d.h>


Tle493d Tle493dMagnetic3DSensor = Tle493d();
void setup() {
  Serial.begin(9600);
  pinMode (3,OUTPUT);
  pinMode (14,OUTPUT);
  digitalWrite (3,LOW);  
  digitalWrite (14,HIGH);    
  while (!Serial);
  
  //If using the MS2Go-Kit: Enable following lines to switch on the sensor
  // ***
   pinMode(LED2, OUTPUT);
   digitalWrite(LED2, HIGH);
   delay(50);
  // ***
    
  Tle493dMagnetic3DSensor.begin();
}


void loop() {
  Tle493dMagnetic3DSensor.updateData();
  Serial.print(Tle493dMagnetic3DSensor.getAzimuth());
  Serial.print("\t");
  if(Tle493dMagnetic3DSensor.getAzimuth()>0) {
    digitalWrite (3,LOW);
    digitalWrite (14,HIGH);
    Serial.println(Tle493dMagnetic3DSensor.getPolar());    
  }
  else {
    Serial.println(-Tle493dMagnetic3DSensor.getPolar());   
    digitalWrite (3,HIGH);
    digitalWrite (14,LOW);    
    delay(1000);
    digitalWrite (3,LOW);
    digitalWrite (14,HIGH); 
  }
}

Below is an image of how to mount the 3D Magnetic Sensor 2Go board:

image

How does it works?

  1. The magnetic sensor knob can turn and it gives me the Azimuth in radians, to its left it gives positive values and to its right negative values;
  2. I have attached a plastic strip to the magnetic sensor knob to move it every time the rubber ball hit it or the servo returns it to the to the starting position;
  3. When a rubber ball hits the plastic strip, the magnetic sensor knob turns and goes into the area of negative radian values;
  4. Then we activate a pulse through pin 3, and this activates the relay. Finally, this relay activates the user button of the PSoC 62S2 WiFi BT Pioneer board;
  5. The PSoC 62S2 WiFi BT Pioneer board performs two functions: 1) activate servo 7 to returns the magnetic sensor knob into the area of positive radian values, and 2) the robot arm takes the rubber ball and places it in the container;
  6. The cycle repeats until there're no more rubber balls; and
  7. This system can be used to detect, count, and sorter objects.

image

 Below I show you the code uploaded to the PSoC 62S2 WiFi BT Pioneer board: sorting_objects.c

// AUTHOR: GUILLERMO PEREZ GUILLEN


#include "cy_pdl.h"
#include "cy_retarget_io.h"
#include "cyhal.h"
#include "cybsp.h"


/******************************************************************************
 * Macros
 *****************************************************************************/
#define DELAY_SHORT_MS          (250)   /* milliseconds */
#define DELAY_LONG_MS           (300)   /* milliseconds */
#define LED_BLINK_COUNT         (4)
#define GPIO_INTERRUPT_PRIORITY (7u)


/* PWM Frequency */
#define PWM_FREQUENCY (50u)


/* PWM Duty-cycle */
#define PWM_DUTY_CYCLE_1 (4.58f) //  30 degrees
#define PWM_DUTY_CYCLE_2 (7.75f) //  90 degrees
#define PWM_DUTY_CYCLE_3 (12.50f) // 180 degrees
#define PWM_DUTY_CYCLE_4 (10.92f) // 150 degrees
#define PWM_DUTY_CYCLE_5 (7.75f) //  90 degrees
#define PWM_DUTY_CYCLE_6 (4.50f) //  30 degrees
#define PWM_DUTY_CYCLE_7 (12.50f) //  30 degrees


/*******************************************************************************
* Function Prototypes
********************************************************************************/
static void gpio_interrupt_handler(void *handler_arg, cyhal_gpio_event_t event);


/*******************************************************************************
* Global Variables
********************************************************************************/
volatile bool gpio_intr_flag = false;


/*******************************************************************************
* Function Name: main
*******************************************************************************/
int main(void)
{
    cy_rslt_t result;
    uint32_t count = 0;
    uint32_t delay_led_blink = DELAY_LONG_MS;


    /* PWM object */
    cyhal_pwm_t servo_1, servo_2, servo_3, servo_4, servo_5, servo_6, servo_7;


    /* Initialize the device and board peripherals */
    result = cybsp_init();
    
    /* Board init failed. Stop program execution */
    if (result != CY_RSLT_SUCCESS)
    {
        CY_ASSERT(0);
    }


    /* Initialize retarget-io to use the debug UART port */
    result = cy_retarget_io_init(CYBSP_DEBUG_UART_TX, CYBSP_DEBUG_UART_RX,
                                 CY_RETARGET_IO_BAUDRATE);


    /* Initialize the user LED */
    result = cyhal_gpio_init(CYBSP_USER_LED, CYHAL_GPIO_DIR_OUTPUT,
                    CYHAL_GPIO_DRIVE_STRONG, CYBSP_LED_STATE_OFF);


    /* Initialize the user button */
    result = cyhal_gpio_init(CYBSP_USER_BTN, CYHAL_GPIO_DIR_INPUT,
                    CYHAL_GPIO_DRIVE_PULLUP, CYBSP_BTN_OFF);


    /* Configure GPIO interrupt */
    cyhal_gpio_register_callback(CYBSP_USER_BTN,
                                 gpio_interrupt_handler, NULL);
    cyhal_gpio_enable_event(CYBSP_USER_BTN, CYHAL_GPIO_IRQ_FALL,
                                 GPIO_INTERRUPT_PRIORITY, true);


    /* Enable global interrupts */
    __enable_irq();


    /* Set the PWM output frequency and duty cycle */
    result = cyhal_pwm_set_duty_cycle(&servo_1, PWM_DUTY_CYCLE_1, PWM_FREQUENCY);
    result = cyhal_pwm_set_duty_cycle(&servo_2, PWM_DUTY_CYCLE_2, PWM_FREQUENCY);
    result = cyhal_pwm_set_duty_cycle(&servo_3, PWM_DUTY_CYCLE_3, PWM_FREQUENCY);
    result = cyhal_pwm_set_duty_cycle(&servo_4, PWM_DUTY_CYCLE_4, PWM_FREQUENCY);
    result = cyhal_pwm_set_duty_cycle(&servo_5, PWM_DUTY_CYCLE_5, PWM_FREQUENCY);
    result = cyhal_pwm_set_duty_cycle(&servo_6, PWM_DUTY_CYCLE_6, PWM_FREQUENCY);
    result = cyhal_pwm_set_duty_cycle(&servo_7, PWM_DUTY_CYCLE_7, PWM_FREQUENCY);


    /* \x1b[2J\x1b[;H - ANSI ESC sequence for clear screen */
    printf("\x1b[2J\x1b[;H");
    printf("**************** PSoC 6 MCU: GPIO Interrupt *****************\r\n");


    for (;;)
    {
        /* Start the PWM */
        result = cyhal_pwm_start(&servo_1);
        cyhal_system_delay_ms(10);
        result = cyhal_pwm_start(&servo_2);
        cyhal_system_delay_ms(10);
        result = cyhal_pwm_start(&servo_3);
        cyhal_system_delay_ms(10);
        result = cyhal_pwm_start(&servo_4);
        cyhal_system_delay_ms(10);
        result = cyhal_pwm_start(&servo_5);
        cyhal_system_delay_ms(10);
        result = cyhal_pwm_start(&servo_6);
        cyhal_system_delay_ms(10);
        result = cyhal_pwm_start(&servo_7);
        cyhal_system_delay_ms(10);


        /* Check the interrupt status */
        if (true == gpio_intr_flag)
        {
            gpio_intr_flag = false;


            /* Update LED toggle delay */
            if (DELAY_LONG_MS == delay_led_blink)
            {
                delay_led_blink = DELAY_SHORT_MS;
            }
            else
            {
                delay_led_blink = DELAY_LONG_MS;
            }
        }


        /* Blink LED four times */
        for (count = 0; count < LED_BLINK_COUNT; count++)
        {
            cyhal_gpio_write(CYBSP_USER_LED, CYBSP_LED_STATE_ON);
            cyhal_system_delay_ms(delay_led_blink);
            cyhal_gpio_write(CYBSP_USER_LED, CYBSP_LED_STATE_OFF);
            cyhal_system_delay_ms(delay_led_blink);
        }
        cyhal_system_delay_ms(10);


        for (int i = 180; i >= 0; i--){ // servo_7
        float PWM_DUTY_CYCLE_G = ((i*9.5)/180)+3;
        result = cyhal_pwm_set_duty_cycle(&servo_7, PWM_DUTY_CYCLE_G, PWM_FREQUENCY);
        result = cyhal_pwm_start(&servo_7);
            cyhal_system_delay_ms(20);
        }
        cyhal_system_delay_ms(100);


        for (int i = 0; i <= 190; i++){ // servo_7
        float PWM_DUTY_CYCLE_G = ((i*9.5)/180)+3;
        result = cyhal_pwm_set_duty_cycle(&servo_7, PWM_DUTY_CYCLE_G, PWM_FREQUENCY);
        result = cyhal_pwm_start(&servo_7);
            cyhal_system_delay_ms(20);
        }
        cyhal_system_delay_ms(100);


        for (int i = 180; i >= 90; i--){ // servo_3
        float PWM_DUTY_CYCLE_C = ((i*9.5)/180)+3;
        result = cyhal_pwm_set_duty_cycle(&servo_3, PWM_DUTY_CYCLE_C, PWM_FREQUENCY);
        result = cyhal_pwm_start(&servo_3);
            cyhal_system_delay_ms(20);
        }
        cyhal_system_delay_ms(100);


        for (int i = 90; i <= 120; i++){ // servo_3
        float PWM_DUTY_CYCLE_C = ((i*9.5)/180)+3;
        result = cyhal_pwm_set_duty_cycle(&servo_3, PWM_DUTY_CYCLE_C, PWM_FREQUENCY);
        result = cyhal_pwm_start(&servo_3);
            cyhal_system_delay_ms(20);
        }
        cyhal_system_delay_ms(100);


        for (int i = 110; i >= 60; i--){ // servo_6
        float PWM_DUTY_CYCLE_F = ((i*9.5)/180)+3;
        result = cyhal_pwm_set_duty_cycle(&servo_6, PWM_DUTY_CYCLE_F, PWM_FREQUENCY);
        result = cyhal_pwm_start(&servo_6);
            cyhal_system_delay_ms(20);
        }
        cyhal_system_delay_ms(100);


        for (int i = 150; i >= 90; i--){ // servo_4
        float PWM_DUTY_CYCLE_D = ((i*9.5)/180)+3;
        result = cyhal_pwm_set_duty_cycle(&servo_4, PWM_DUTY_CYCLE_D, PWM_FREQUENCY);
        result = cyhal_pwm_start(&servo_4);
            cyhal_system_delay_ms(20);
        }
        cyhal_system_delay_ms(100);


        for (int i = 180; i >= 90; i--){ // servo_1 ***
        float PWM_DUTY_CYCLE_A = ((i*9.5)/180)+3;
        result = cyhal_pwm_set_duty_cycle(&servo_1, PWM_DUTY_CYCLE_A, PWM_FREQUENCY);
        result = cyhal_pwm_start(&servo_1);
            cyhal_system_delay_ms(20);
        }
        cyhal_system_delay_ms(100);


        for (int i = 90; i <= 120; i++){ // servo_4
        float PWM_DUTY_CYCLE_D = ((i*9.5)/180)+3;
        result = cyhal_pwm_set_duty_cycle(&servo_4, PWM_DUTY_CYCLE_D, PWM_FREQUENCY);
        result = cyhal_pwm_start(&servo_4);
            cyhal_system_delay_ms(20);
        }
        cyhal_system_delay_ms(100);


        for (int i = 60; i <= 110; i++){ // servo_6
        float PWM_DUTY_CYCLE_F = ((i*9.5)/180)+3;
        result = cyhal_pwm_set_duty_cycle(&servo_6, PWM_DUTY_CYCLE_F, PWM_FREQUENCY);
        result = cyhal_pwm_start(&servo_6);
            cyhal_system_delay_ms(20);
        }
        cyhal_system_delay_ms(100);


        for (int i = 90; i >= 60; i--){ // servo_1 ***
        float PWM_DUTY_CYCLE_A = ((i*9.5)/180)+3;
        result = cyhal_pwm_set_duty_cycle(&servo_1, PWM_DUTY_CYCLE_A, PWM_FREQUENCY);
        result = cyhal_pwm_start(&servo_1);
            cyhal_system_delay_ms(20);
        }
        cyhal_system_delay_ms(100);


        for (int i = 120; i <= 150; i++){ // servo_4
        float PWM_DUTY_CYCLE_D = ((i*9.5)/180)+3;
        result = cyhal_pwm_set_duty_cycle(&servo_4, PWM_DUTY_CYCLE_D, PWM_FREQUENCY);
        result = cyhal_pwm_start(&servo_4);
            cyhal_system_delay_ms(20);
        }
        cyhal_system_delay_ms(100);


        for (int i = 120; i >= 90; i--){ // servo_3
        float PWM_DUTY_CYCLE_C = ((i*9.5)/180)+3;
        result = cyhal_pwm_set_duty_cycle(&servo_3, PWM_DUTY_CYCLE_C, PWM_FREQUENCY);
        result = cyhal_pwm_start(&servo_3);
            cyhal_system_delay_ms(20);
        }
        cyhal_system_delay_ms(100);


        for (int i = 75; i <= 90; i++){ // servo_2
        float PWM_DUTY_CYCLE_B = ((i*9.5)/180)+3;
        result = cyhal_pwm_set_duty_cycle(&servo_2, PWM_DUTY_CYCLE_B, PWM_FREQUENCY);
        result = cyhal_pwm_start(&servo_2);
            cyhal_system_delay_ms(20);
        }
        cyhal_system_delay_ms(100);


        for (int i = 110; i >= 30; i--){ // servo_6
        float PWM_DUTY_CYCLE_F = ((i*9.5)/180)+3;
        result = cyhal_pwm_set_duty_cycle(&servo_6, PWM_DUTY_CYCLE_F, PWM_FREQUENCY);
        result = cyhal_pwm_start(&servo_6);
            cyhal_system_delay_ms(20);
        }
        cyhal_system_delay_ms(100);


        for (int i = 60; i >= 30; i--){ // servo_1 ***
        float PWM_DUTY_CYCLE_A = ((i*9.5)/180)+3;
        result = cyhal_pwm_set_duty_cycle(&servo_1, PWM_DUTY_CYCLE_A, PWM_FREQUENCY);
        result = cyhal_pwm_start(&servo_1);
            cyhal_system_delay_ms(20);
        }
        cyhal_system_delay_ms(100);


        for (int i = 90; i <= 180; i++){ // servo_3
        float PWM_DUTY_CYCLE_C = ((i*9.5)/180)+3;
        result = cyhal_pwm_set_duty_cycle(&servo_3, PWM_DUTY_CYCLE_C, PWM_FREQUENCY);
        result = cyhal_pwm_start(&servo_3);
            cyhal_system_delay_ms(20);
        }
        cyhal_system_delay_ms(10);


        result = cyhal_pwm_stop(&servo_1);
        result = cyhal_pwm_stop(&servo_2);
        result = cyhal_pwm_stop(&servo_3);
        result = cyhal_pwm_stop(&servo_4);
        result = cyhal_pwm_stop(&servo_5);
        result = cyhal_pwm_stop(&servo_6);
        result = cyhal_pwm_stop(&servo_7);
        cyhal_system_delay_ms(10);


        /* Enter deep sleep mode */
        cyhal_system_deepsleep();
    }
}


/*******************************************************************************
* Function Name: gpio_interrupt_handler
*******************************************************************************/


static void gpio_interrupt_handler(void *handler_arg, cyhal_gpio_irq_event_t event)
{
    gpio_intr_flag = true;
}

In the video below I show you a test with the 3D Magnetic Sensor 2Go TLE493D, the robot arm, and the PSoC 62S2 WiFi BT Pioneer Board.

Conclusión:

  1. In a creative way, here I have used the 3D Magnetic Sensor 2Go TLE493D to detect the rubber balls and warn the PSoC 6 WiFi-BT Pioneer Kit board that it has to activate the robot arm to take the ball and place it in a container. The robot arm goes into low energy mode once this task is finished.
  2. In addition, a servo is activated to return the magnetic sensor to its original position.
  3. My idea is to demonstrate the possibility of helping to classify objects if we combine this prototype with the two versions previously shown in this project: the Arduino version, and the first version of Infineon.

Discussions