Robotic Arm Controlled with Capsense

1. Introduction

Robotic arms are used in many industrial applications, such as picking and placing goods or products, material handling, welding, inspection, etc. This solution will use Capsense technology to perform these tasks manually and independently, controlling each degree of freedom just like using a joystick.

In my case, I'm talkink about a cobot for electronic workshop, which is a collaborative robot or a robot intended for direct human robot interaction within a shared space, or where humans and robots are in close proximity. Cobot safety may rely on lightweight construction materials, rounded edges, and inherent limitation of speed and force, or on sensors and software that ensure safe behavior. Reference: https://en.wikipedia.org/wiki/Cobot

2. Getting Started

In this project we will use ModusToolbox 3.0 which has enhanced support for multi-core project workflow. The release features dual-core device support, a new graphical tool for customer board support package (BSP) development, infrastructure support for ModusToolbox Packs and backend system improvements.

About ModusToolbox software you can find the documentation here: https://www.infineon.com/cms/en/design-support/tools/sdk/modustoolbox-software/

The PSoC™ 62S2 Pioneer Kit features the PSoC™ 62 MCU (CY8C624ABZI-S2D44): 150-MHz Arm Cortex-M4 and 100-MHz Arm Cortex-M0+ cores, 2MB of Flash, 1MB of SRAM, Secure Digital Host Controller (SDHC) supporting SD/SDIO/eMMC interfaces, programmable analog blocks, programmable digital blocks, Full-Speed USB, a serial memory interface, a PDM-PCM digital microphone interface, and industry-leading capacitive-sensing with CAPSENSE™.

Documentation: https://www.infineon.com/cms/en/product/evaluation-boards/cy8ckit-062s2-43012/

CAPSENSE™ technology:

• Doesn't involve moving parts and will not wear out over time.
• Can be completely sealed to prevent moisture from seeping in.
• Doesn't require force to operate.
• Results in reduced BOM cost.
• Offers more flexibility in button shape, size, and graphical representation for your overall design.

3. Controlling Servo Motor With ModusToolbox

Servo motors are electromechanical devices that have the ability to control the angular position of their axis, their operation consists of receiving the information of the desired angle through the pulse width of a PWM signal to bring the axis to said position. According to the datasheet of the SG995 we have this graph that helps us to control the PWM signal and the duty cycle.

The position of the axis of the motor depends on the duty cycle of the signal. There are some standard calculations for degree rotation. If the PWM signal is high for 0.5ms in a single cycle, the axis moves to zero degrees. To rotate the motor axis to 90 degrees, the signal should be high for 1.5ms. Similarly, a 2.5ms ON-time signal leads to 180-degree axial position. In this way, we can measure and control our servo motor to desired degrees.

According to the example "HAL PWM square wave" we see that it uses a library to control the PWM signal and the duty cycle. I just have to insert the values of the frequency and the duty cycle percentage. By interpolation, we can calculate: if 20 ms is 100% duty cycle, then 0.5 ms is 2.5% duty cycle. Similary 1.5 ms is 7.5% duty cycle, and 2.5 ms is 12.5% duty cycle.

I experimented with this data, and the only value I adjusted was the 0° angle at 3% duty cycle, because the servo was vibrating and making noise at 2.5%. The final data is shown below:

Fitting a data set to a trend line using excel

The data between the angle and the duty cycle percentage are not linear, so I found a method to get a formula with excel.

• First we make a scatterplot using the data from the table above. The x value corresponds to the angle, and the y value corresponds to the duty cycle percentage.
• Then, on a point of the graph we click with the right mouse button and select add trendline.
• In the drop down menu I chose polynomial trendline option with degree 2.
• Finally, through the displayed menu I ask Excel to display equation on chart. Below I show you the final result.

This is the formula that I will use in the next sections to move each servo of the robotic arm.

4. Soldering Components Using Capsense

In this section I will show you how to use the CAPSENSE technology to solder components, and using the PSoC 62S2 WiFi BT Pioneer Kit board. Below I show you the schematic diagram.

Schematic Diagram

How does it work?

• Every time I need to hold a PCB to solder a component, I simply have to move my finger on the sliding bar to SLD 4 position. Then the gripper-1 is closed.

• When I need to release a PCB after soldering a component, I simply have to move my finger on the sliding bar to SLD 0 position. Then the gripper-1 is opened.

• The pinout of this board is shown below.

Programming With ModusToolbox

Here I have used the "CAPSENSE_Buttons_and_Slider" example. First select the board in: File - New - ModusToolbox Application

So I changed the application name to Soldering_Components_Using_Capsense as shown below:

Once the project is created, you have to change the code of the led.c file to:

// AUTHOR: Guillermo Perez Guillen

/*******************************************************************************
* Header files includes
*******************************************************************************/
#include "cybsp.h"
#include "cyhal.h"
#include "led.h"
#include <stdio.h> // added
#include <math.h>

/*******************************************************************************
* Global constants
*******************************************************************************/
#define PWM_LED_FREQ_HZ    (1000000lu)  /* in Hz */
#define GET_DUTY_CYCLE(x)    (100 - x)


/******************************************************************************
 * Servo Macros - added
 *****************************************************************************/

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

/* PWM Duty-cycle */
#define PWM_DUTY_CYCLE_1 (4.58f) //  30 degrees

/*******************************************************************************
* Global constants
*******************************************************************************/
led_state_t led_state_cur = LED_OFF;
cyhal_pwm_t pwm_led;
cyhal_pwm_t servo_1; // added

/*******************************************************************************
* Function Name: update_led_state
********************************************************************************
* Summary:
*  This function updates the LED state, based on the touch input.
*
* Parameter:
*  ledData: the pointer to the LED data structure
*
*******************************************************************************/
void update_led_state(led_data_t *ledData)
{
    if ((led_state_cur == LED_OFF) && (ledData->state == LED_ON))
    {
        cyhal_pwm_start(&pwm_led);
        led_state_cur = LED_ON;
        ledData->brightness = LED_MAX_BRIGHTNESS;
        //printf("brightness high!!!\r\n\n");
    }
    else if ((led_state_cur == LED_ON) && (ledData->state == LED_OFF))
    {
        cyhal_pwm_stop(&pwm_led);
        led_state_cur = LED_OFF;
        ledData->brightness = 0;
        //printf("brightness low!!!\r\n\n");
    }
    else
    {
    }

    if ((LED_ON == led_state_cur) || ((LED_OFF == led_state_cur) && (ledData->brightness > 0)))
    {
        cyhal_pwm_start(&pwm_led);
        uint32_t brightness = (ledData->brightness < LED_MIN_BRIGHTNESS) ? LED_MIN_BRIGHTNESS : ledData->brightness;

        uint32_t servo_control_gripper_1 = brightness;
        uint32_t PWM_DUTY_CYCLE_GRIPPER_1 = 0.00003 * pow(servo_control_gripper_1, 2) + 0.0472 * servo_control_gripper_1 + 3;

        /* Drive the LED with brightness */
        cyhal_pwm_set_duty_cycle(&pwm_led, GET_DUTY_CYCLE(brightness),
                                 PWM_LED_FREQ_HZ);
        cyhal_pwm_set_duty_cycle(&servo_1, PWM_DUTY_CYCLE_GRIPPER_1, PWM_FREQUENCY); // robot gripper
        cyhal_pwm_start(&servo_1); // robot gripper

        led_state_cur = LED_ON;
    }
}

/*******************************************************************************
* Function Name: initialize_led
********************************************************************************
* Summary:
*  Initializes a PWM resource for driving an LED.
*
*******************************************************************************/
cy_rslt_t initialize_led(void)
{
    cy_rslt_t rslt;

    rslt = cyhal_pwm_init(&pwm_led, CYBSP_USER_LED, NULL);
    rslt = cyhal_pwm_init(&servo_1, P7_5, NULL); // added


    if (CY_RSLT_SUCCESS == rslt)
    {
        rslt = cyhal_pwm_set_duty_cycle(&pwm_led,
                                        GET_DUTY_CYCLE(LED_MAX_BRIGHTNESS),
                                        PWM_LED_FREQ_HZ);
        if (CY_RSLT_SUCCESS == rslt)
        {
            rslt = cyhal_pwm_start(&pwm_led);
        }
    }

    if (CY_RSLT_SUCCESS == rslt)
    {
        led_state_cur = LED_ON;
    }
    return rslt;
}

Here I have used as a reference the brightness of the LED, which goes from 0 to 100 and is controlled by the Capsense slider. So, the gripper servo will also move from zero to 100 degrees and will be controlled by Capsense itself, in fact the gripper can't be opened beyond 110 degrees and it's perfect to control it. The formula used as a reference is the one calculated in section 3 of this tutorial.

uint32_t brightness = (ledData->brightness < LED_MIN_BRIGHTNESS) ? LED_MIN_BRIGHTNESS : ledData->brightness;
        uint32_t servo_control_gripper_1 = brightness;
        uint32_t PWM_DUTY_CYCLE_GRIPPER_1 = 0.00003 * pow(servo_control_gripper_1, 2) + 0.0472 * servo_control_gripper_1 + 3;

        /* Drive the LED with brightness */
        cyhal_pwm_set_duty_cycle(&pwm_led, GET_DUTY_CYCLE(brightness),
                                 PWM_LED_FREQ_HZ);
        cyhal_pwm_set_duty_cycle(&servo_1, PWM_DUTY_CYCLE_GRIPPER_1, PWM_FREQUENCY); // robot gripper
        cyhal_pwm_start(&servo_1); // robot gripper

Upload the code to the board with Run - Run Configurations and select Soldering_Components_Using_Capsense Program (KitPro3MiniProg4) as shown below:

Test

5. Unsoldering Components Using Capsense

Now, I show you how to use the CAPSENSE technology to unsolder components, and again using the PSoC 62S2 WiFi BT Pioneer Kit board. Below I show you the schematic diagram.

Schematic Diagram

How does it work?

• Here I will use the device created in section 3. This gripper-1 will be used to hold a part of the board on which I will desolder a component.

• When I turn on the system the robot arm is positioned according to the initial configuration. That is, servo 1 moves to 30 degrees from the origin.

• When I press the capsense BTN0 button, servo 1 moves to 165 degrees from the origin to hold the PCB with gripper-2. Next, servo 1 moves to 100 degrees from the origin to face gripper 2 to gripper-1. Now I can hold the PCB with both grippers as shown in the attached image.

• Since the PCB board is firmly holded, then I can desolder components comfortably. When I finish desoldering I can open the griper-1.

• When I press the BTN1 capsense button, servo 1 moves from 100 degrees from the origin to 30 degrees. Here the robot arm releases the PCB board and returns to the position indicated in point 2.
• Additionally, I have configured servo 4 to rotate the gripper with the Capsense sliding bar. This movement can only be done when the robot arm is in the initial position indicated in point 2. When I move my finger to the SLD 0 position, the gripper-2 tilts 90 degrees from the origin.

Prgramming With ModusToolbox

Again as in section 4, I have used the "CAPSENSE_Buttons_and_Slider" example and named the project as Unsoldering_Components_Using_Capsense.

Finally, I have modified the "led.c" code. Below I show you the full code.

// AUTHOR: Guillermo Perez Guillen

/*******************************************************************************
* Header files includes
*******************************************************************************/
#include "cybsp.h"
#include "cyhal.h"
#include "led.h"
#include <stdio.h> // added
#include <math.h>

/*******************************************************************************
* Global constants
*******************************************************************************/
#define PWM_LED_FREQ_HZ    (1000000lu)  /* in Hz */
#define GET_DUTY_CYCLE(x)    (100 - x)

/*******************************************************************************
* Global constants
*******************************************************************************/
led_state_t led_state_cur = LED_OFF;
cyhal_pwm_t pwm_led;
cyhal_pwm_t servo_1; // robot arm
cyhal_pwm_t servo_2;
cyhal_pwm_t servo_3;
cyhal_pwm_t servo_4;
cyhal_pwm_t servo_5; // robot gripper-2

/******************************************************************************
 * Servo Macros - added
 *****************************************************************************/

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

/* PWM Duty-cycle */
#define PWM_DUTY_CYCLE_1 (4.44f) //  30 degrees
#define PWM_DUTY_CYCLE_2 (8.02f) //  100 degrees
#define PWM_DUTY_CYCLE_3 (10.76f) // 150 degrees
#define PWM_DUTY_CYCLE_4 (8.02f) //  100 degrees
#define PWM_DUTY_CYCLE_5 (3.48f) //  10 degrees - robot gripper-2

/*******************************************************************************
* Function Name: update_led_state
********************************************************************************
* Summary:
*  This function updates the LED state, based on the touch input.
*
* Parameter:
*  ledData: the pointer to the LED data structure
*
*******************************************************************************/
void update_led_state(led_data_t *ledData)
{

    if ((led_state_cur == LED_OFF) && (ledData->state == LED_ON))
    {
        cyhal_pwm_start(&pwm_led);
        led_state_cur = LED_ON;
        ledData->brightness = LED_MAX_BRIGHTNESS;

        for (int i = 65; i <= 90; i++){ // servo_2 ***
        float PWM_DUTY_CYCLE_S2 = 0.00003 * pow(i, 2) + 0.0472 * i + 3;
        cyhal_pwm_set_duty_cycle(&servo_2, PWM_DUTY_CYCLE_S2, PWM_FREQUENCY);
        cyhal_pwm_start(&servo_2);
        cyhal_system_delay_ms(20);
        }
        cyhal_system_delay_ms(50);

        for (int i = 100; i >= 30; i--){ // servo_1 ***
        float PWM_DUTY_CYCLE_S1 = 0.00003 * pow(i, 2) + 0.0472 * i + 3;
        cyhal_pwm_set_duty_cycle(&servo_1, PWM_DUTY_CYCLE_S1, PWM_FREQUENCY);
        cyhal_pwm_start(&servo_1);
        cyhal_system_delay_ms(20);
        }
        cyhal_system_delay_ms(50);

        for (int i = 90; i >= 65; i--){ // servo_2 ***
        float PWM_DUTY_CYCLE_S2 = 0.00003 * pow(i, 2) + 0.0472 * i + 3;
        cyhal_pwm_set_duty_cycle(&servo_2, PWM_DUTY_CYCLE_S2, PWM_FREQUENCY);
        cyhal_pwm_start(&servo_2);
        cyhal_system_delay_ms(20);
        }
        cyhal_system_delay_ms(50);

        for (int i = 8; i <= 60; i++){ // servo_5 *** robot gripper-2 opened
        float PWM_DUTY_CYCLE_S5 = 0.00003 * pow(i, 2) + 0.0472 * i + 3;
        cyhal_pwm_set_duty_cycle(&servo_5, PWM_DUTY_CYCLE_S5, PWM_FREQUENCY);
        cyhal_pwm_start(&servo_5);
        cyhal_system_delay_ms(20);
        }
        cyhal_system_delay_ms(500);

        for (int i = 65; i <= 90; i++){ // servo_2 ***
        float PWM_DUTY_CYCLE_S2 = 0.00003 * pow(i, 2) + 0.0472 * i + 3;
        cyhal_pwm_set_duty_cycle(&servo_2, PWM_DUTY_CYCLE_S2, PWM_FREQUENCY);
        cyhal_pwm_start(&servo_2);
        cyhal_system_delay_ms(20);
        }
        cyhal_system_delay_ms(250);

        for (int i = 60; i >= 10; i--){ // servo_5 *** robot gripper-2 closesd
        float PWM_DUTY_CYCLE_S5 = 0.00003 * pow(i, 2) + 0.0472 * i + 3;
        cyhal_pwm_set_duty_cycle(&servo_5, PWM_DUTY_CYCLE_S5, PWM_FREQUENCY);
        cyhal_pwm_start(&servo_5);
        cyhal_system_delay_ms(20);
        }
        cyhal_system_delay_ms(50);

    }
    else if ((led_state_cur == LED_ON) && (ledData->state == LED_OFF))
    {
        cyhal_pwm_stop(&pwm_led);
        led_state_cur = LED_OFF;
        ledData->brightness = 0;

        for (int i = 30; i <= 165; i++){ // servo_1 ***
        float PWM_DUTY_CYCLE_S1 = 0.00003 * pow(i, 2) + 0.0472 * i + 3;
        cyhal_pwm_set_duty_cycle(&servo_1, PWM_DUTY_CYCLE_S1, PWM_FREQUENCY);
        cyhal_pwm_start(&servo_1);
        cyhal_system_delay_ms(20);
        }
        cyhal_system_delay_ms(250);

        for (int i = 10; i <= 60; i++){ // servo_5 *** robor gripper-2 opened
        float PWM_DUTY_CYCLE_S5 = 0.00003 * pow(i, 2) + 0.0472 * i + 3;
        cyhal_pwm_set_duty_cycle(&servo_5, PWM_DUTY_CYCLE_S5, PWM_FREQUENCY);
        cyhal_pwm_start(&servo_5);
        cyhal_system_delay_ms(20);
        }
        cyhal_system_delay_ms(50);

        for (int i = 90; i >= 65; i--){ // servo_2 ***
        float PWM_DUTY_CYCLE_S2 = 0.00003 * pow(i, 2) + 0.0472 * i + 3;
        cyhal_pwm_set_duty_cycle(&servo_2, PWM_DUTY_CYCLE_S2, PWM_FREQUENCY);
        cyhal_pwm_start(&servo_2);
        cyhal_system_delay_ms(20);
        }
        cyhal_system_delay_ms(500);

        for (int i = 60; i >= 8; i--){ // servo_5 *** robot gripper-2 closed
        float PWM_DUTY_CYCLE_S5 = 0.00003 * pow(i, 2) + 0.0472 * i + 3;
        cyhal_pwm_set_duty_cycle(&servo_5, PWM_DUTY_CYCLE_S5, PWM_FREQUENCY);
        cyhal_pwm_start(&servo_5);
        cyhal_system_delay_ms(20);
        }
        cyhal_system_delay_ms(250);

        for (int i = 65; i <= 90; i++){ // servo_2 ***
        float PWM_DUTY_CYCLE_S2 = 0.00003 * pow(i, 2) + 0.0472 * i + 3;
        cyhal_pwm_set_duty_cycle(&servo_2, PWM_DUTY_CYCLE_S2, PWM_FREQUENCY);
        cyhal_pwm_start(&servo_2);
        cyhal_system_delay_ms(20);
        }
        cyhal_system_delay_ms(50);

        for (int i = 165; i >= 100; i--){ // servo_1 ***
        float PWM_DUTY_CYCLE_S1 = 0.00003 * pow(i, 2) + 0.0472 * i + 3;
        cyhal_pwm_set_duty_cycle(&servo_1, PWM_DUTY_CYCLE_S1, PWM_FREQUENCY);
        cyhal_pwm_start(&servo_1);
        cyhal_system_delay_ms(20);
        }
        cyhal_system_delay_ms(250);

        for (int i = 90; i >= 65; i--){ // servo_2 ***
        float PWM_DUTY_CYCLE_S2 = 0.00003 * pow(i, 2) + 0.0472 * i + 3;
        cyhal_pwm_set_duty_cycle(&servo_2, PWM_DUTY_CYCLE_S2, PWM_FREQUENCY);
        cyhal_pwm_start(&servo_2);
        cyhal_system_delay_ms(20);
        }
        cyhal_system_delay_ms(50);
    }
    else
    {
    }

    if ((LED_ON == led_state_cur) || ((LED_OFF == led_state_cur) && (ledData->brightness > 0)))
{
        cyhal_pwm_start(&pwm_led);
        uint32_t brightness = (ledData->brightness < LED_MIN_BRIGHTNESS) ? LED_MIN_BRIGHTNESS : ledData->brightness;

        uint32_t servo_control_gripper_2 = brightness;
        uint32_t PWM_DUTY_CYCLE_GRIPPER_2 = 0.00003 * pow(servo_control_gripper_2, 2) + 0.0472 * servo_control_gripper_2 + 3;


                /* Drive the LED with brightness */
        cyhal_pwm_set_duty_cycle(&pwm_led, GET_DUTY_CYCLE(brightness), PWM_LED_FREQ_HZ);
        cyhal_pwm_set_duty_cycle(&servo_4, PWM_DUTY_CYCLE_GRIPPER_2, PWM_FREQUENCY); // servo 4
        cyhal_pwm_start(&servo_4);

        led_state_cur = LED_ON;
    }
}

/*******************************************************************************
* Function Name: initialize_led
********************************************************************************
* Summary:
*  Initializes a PWM resource for driving an LED.
*
*******************************************************************************/
cy_rslt_t initialize_led(void)
{
    cy_rslt_t rslt;

    rslt = cyhal_pwm_init(&pwm_led, CYBSP_USER_LED, NULL);
    rslt = cyhal_pwm_init(&servo_1, P7_5, NULL); // pinout
    rslt = cyhal_pwm_init(&servo_2, P7_6, NULL);
    rslt = cyhal_pwm_init(&servo_3, P12_3, NULL);
    rslt = cyhal_pwm_init(&servo_4, P12_0, NULL);
    rslt = cyhal_pwm_init(&servo_5, P12_1, NULL);

    cyhal_pwm_set_duty_cycle(&servo_1, PWM_DUTY_CYCLE_1, PWM_FREQUENCY);
    cyhal_pwm_start(&servo_1);
    cyhal_pwm_set_duty_cycle(&servo_2, PWM_DUTY_CYCLE_2, PWM_FREQUENCY);
    cyhal_pwm_start(&servo_2);
    cyhal_pwm_set_duty_cycle(&servo_3, PWM_DUTY_CYCLE_3, PWM_FREQUENCY);
    cyhal_pwm_start(&servo_3);
    cyhal_pwm_set_duty_cycle(&servo_4, PWM_DUTY_CYCLE_4, PWM_FREQUENCY);
    cyhal_pwm_start(&servo_4);
    cyhal_pwm_set_duty_cycle(&servo_5, PWM_DUTY_CYCLE_5, PWM_FREQUENCY);
    cyhal_pwm_start(&servo_5);

    if (CY_RSLT_SUCCESS == rslt)
    {
        rslt = cyhal_pwm_set_duty_cycle(&pwm_led,
                                        GET_DUTY_CYCLE(LED_MAX_BRIGHTNESS),
                                        PWM_LED_FREQ_HZ);
        if (CY_RSLT_SUCCESS == rslt)
        {
            rslt = cyhal_pwm_start(&pwm_led);
        }
        
    }

    if (CY_RSLT_SUCCESS == rslt)
    {
        led_state_cur = LED_ON;
    }

    return rslt;
}

As in the previous section, I have also used Capsense sliding to control the gripper. Again, all other servos are controlled by the formula obtained in section 3, for example, servo 2 is moved from 65° to 90° with:

for (int i = 65; i <= 90; i++){ // servo_2 ***
        float PWM_DUTY_CYCLE_S2 = 0.00003 * pow(i, 2) + 0.0472 * i + 3;
        cyhal_pwm_set_duty_cycle(&servo_2, PWM_DUTY_CYCLE_S2, PWM_FREQUENCY);
        cyhal_pwm_start(&servo_2);
        cyhal_system_delay_ms(20);
        }

Finally, upload the code as shown in section 4 and enjoy.

6. Test

7. BOM Cost

Below I show you a estimate BOM cost. However you can reduce the total cost by replacing the rechargeable battery with a power supply.

The Excel document can be downloaded from the "CAD - Enclosures and custom parts" section or project repository ( BOM-robotic-arm.xlsx)

8. Conclusion

• In this project I demonstrate the use of capsense sensors in the soldering and desoldering process of a PCB.
• I have used two Infineon PSoC™ 62S2 Wi-Fi BT Pioneer Kit boards, and they were programmed with ModusToolbox 3.0
• This project inspired me to develop the project: Object Classifier Controlled with Capsense