Robotino and Cobotta bCaps Integration

From RobotinoWiki

Introduction

This class uses a b-Cap client to connect to a denso robotics Cobotta.

It provides several methods to control the Cobotta such as

  • Connect to Cobotta
  • Start/Stop CobottaWorld program
  • Read/Write IOs
  • Get Cobotta error status, description and info
  • Reset Cobotta error
  • Get Cobotta status information
  • Get/Set Cobotta override
  • Get a list of robot programs

Preparation

Before you can use this class it is mandatory to do some preparation on the Cobotta. Refer to the manual. Prepare a program called "HOME" in CobottaWorld app. Than navigate to Menu -> Settings -> Execute job from I/O. This will create a file called "Job_PCS.h". You may see it when you connect to Cobotta using RemoteTP app. The "Job_PCS.h" allows the class to call CobottaWorld programs. It is neccessary to perform this aktion once before you can start a new created CobottaWorld program.

Public methods

__init__(host,port=5007,timeout=2000)

The contructor of this class is always executed when the class is being initiated. A connection to the cobotta is established. Also a background task is started wich is used for cyclically monitoring.

Arguments:
host: (str) IP address of cobotta
port: (int) TCP communication port -- default: 5007
timeout: (int) time to connection timeout [10ms] -- default 2000

Example

    Cobotta = cobottabcap("172.21.0.30")

MotionPreparation()

This method sould be called once after switching on the Cobotta or if the Cobotta had a hard collision. The Cobotta prepares it's system for operation and drives to the CALSET position. Make sure that there are no obstacles around the Cobotta before running the motion preperation. Refer to manual ID : 7291 for detaild information about Cobotta startup. After axis calibration the "HOME" program is started. HOME program has to be defined in CobottaWorld app.

Example

    Cobotta = cobottabcap("172.21.0.30")
    Cobotta.MotionPreparation()

UpdateProgramList()

Get the program names of CobottaWorld programs and returns them as string array. The update is also done during the instantiation. If your program isn't listed you may need to update the "Job_PCS.h" file.

Returns:
(str[]) job names

Example

    Cobotta = cobottabcap("172.21.0.30")
    prgnames = Cobotta.UpdateProgramList()

StartHome()

Starts the HOME program. HOME program has to be defined in CobottaWorld app. This method is also used internally to drive to HOME position in MotionPreparation method.

Example

    Cobotta = cobottabcap("172.21.0.30")
    Cobotta.StartHome()

StartProgram(progname,mode)

Starts a CobottaWorld robot program. The program name is handled internally as upper case.

Arguments:
progname: (str) name of CobottaWorld robot program
mode: (int) execution mode
1:One cycle execution
2:Continuous execution
3:Step forward

Example

    Cobotta = cobottabcap("172.21.0.30")
    Cobotta.StartProgram("Main",1)

ResumeProgram()

Continues a CobottaWorld robot program. E.g. after error reset. The program has to be started with "StartProgram" method, first.

Example

    Cobotta = cobottabcap("172.21.0.30")
    Cobotta.ResumeProgram()


StopPrograms()

Stops all running robot programs. Programs will be stopped immediately.

Example

    Cobotta = cobottabcap("172.21.0.30")
    Cobotta.StopPrograms()

CycleEnd()

End all running robot programs. Programs will be stopped at the end of the program.

Example

    Cobotta = cobottabcap("172.21.0.30")
    Cobotta.CycleEnd()

ResetError()

Resets all errors of Cobotta.

Example

    Cobotta = cobottabcap("172.21.0.30")
    Cobotta.ResetError()

CheckIfError()

Check if Cobotta is in error state.

Returns:
(bool) error state; true = Cobotta in error state

Example

    Cobotta = cobottabcap("172.21.0.30")
    haserror = Cobotta.CheckIfError()
    
    if haserror:
        ...

GetErrorCode()

Read the Cobotta hex value error code.

Returns:
(str) error code

Example

    Cobotta = cobottabcap("172.21.0.30")
    errorcode = Cobotta.GetErrorCode()

GetErrorText()

Read the short description of Cobotta error.

Returns:
(str) error description

Example

    Cobotta = cobottabcap("172.21.0.30")
    errortext = Cobotta.GetErrorText()

GetErrorInfo()

Read the Cobotta error info. A more detailed description of Cobotta errors.

Returns:
(str) error info

Example

    Cobotta = cobottabcap("172.21.0.30")
    errortext = Cobotta.GetErrorInfo()

GetSetOverride(ovrd)

Sets the Cobotta override. Reads current override.

Arguments:
ovrd: (int) override to be set; 1..100 (%)
Returns:
(float) current override

Example

    Cobotta = cobottabcap("172.21.0.30")
    override = Cobotta.GetSetOverride(50)

GetStatus()

Read status variables from Cobotta. It is handled as array for easier output from a RobotinoView python script block.

Returns:
(bool[]) current states;
[0]: emergency stop staus; true = emergency stop active
[1]: busy status; true = task is running
[2]: normal status; true = no error
[3]: servo status; true = motors ON

Example

    Cobotta = cobottabcap("172.21.0.30")
    status = Cobotta.GetStatus()
    RV.writeFloatVector(1,status)

GetIO(number)

Read IO variable from Cobotta.

Arguments:
number: (int) number of IO
Returns:
(bool) IO variable value

Example

    Cobotta = cobottabcap("172.21.0.30")
    value = Cobotta.GetIO(128)

SetIO(number,value)

Write IO variable to Cobotta.

Arguments:
number: (int) number of IO
value: (bool) value of IO variable

Example

    Cobotta = cobottabcap("172.21.0.30")
    Cobotta.SetIO(128,True)


RobotinoView example

import sys, io
from pybcapclient import bcapclient as bcapclient
from cobottabcap.cobotta import cobottabcap

sys.stdout = io.StringIO()
sys.stderr = io.StringIO()

###### GLOBAL PARAMETERS ######
COBOTTA_IP = "172.21.0.30"
COBOTTA = None

def start(RV):
    global COBOTTA_IP
    global COBOTTA
    # initiation of class and connect to cobotta
    COBOTTA = cobottabcap(COBOTTA_IP)

    # clear errors
    COBOTTA.ResetError()

    # do motion preparation
    COBOTTA.MotionPreparation()

    # start "MAIN" program
    COBOTTA.StartProgram("MAIN")
	
def step(RV):
    global COBOTTA

    # get status
    status = COBOTTA.GetStatus()
    RV.writeFloatVector(1,status)

    #print('get set override')
    OvrdSoll = RV.readFloat(1)
    OvrdIst = COBOTTA.GetSetOverride(OvrdSoll)
    RV.writeFloat(2,OvrdIst)


    ### Error handling ###
    
    if 1 == status[0] or 0 == status[2]:
        #print('error handling')
        errorcode = COBOTTA.GetErrorCode()
        errortext = COBOTTA.GetErrorText()
        RV.writeString(3,errorcode)
        RV.writeString(4,errortext)
 
    reset = RV.readFloat(2)
    if 1 == reset:
        print('reset')
        RV.writeString(3,'')
        RV.writeString(4,'')
        COBOTTA.ResetError()
        COBOTTA.StartProgram("MAIN")

    def stop(RV):
        print('stop')
        #COBOTTA.StopPrograms()
        COBOTTA.CycleEnd()

    def cleanup(RV):
        print('cleanup')


Change Log

This are the changes in cobottabcap class.
The version of your class is printed in output window during instantiation of this class.
You can also check __version__ attribute.

Date Version Changes
22.01.2021 1.0.0 first Release