ssg48.py

import numpy as np
import time
import Spectral_BLDC as Spectral


class SSG48Gripper:

    OPEN_CLOSE_MARGIN = 2


    def __init__(self, port="can0"):
        try:
            self.communication1 = Spectral.CanCommunication(bustype="socketcan", channel=port, bitrate=1000000)
            self.motor1 = Spectral.SpectralCAN(node_id=0, communication=self.communication1)
            print("Initializing SSG48 Gripper!")
        except:
            print("CAN Connection to SSG gripper failed!")

        self.calibrate()
        self.most_recent_data = None

    def calibrate(self):
        print("Calibrating gripper - warning, will close gripper")
        # Do not deactivate gripper here, this will cause calibration to sometimes fail. You can tell because the gripper will read a current of around -40mA in the get_gripper_data below.
        time.sleep(0.5)
        self.motor1.Send_gripper_calibrate()
        # wait for calibration to finish
        time.sleep(3)
        self.activate_gripper()
        time.sleep(0.5)
        initial_gripper_data = self.get_gripper_data()
        print("Initial gripper data:")
        print(initial_gripper_data)
        if initial_gripper_data["error_bit"] == 1:
            print("The error bit seems to be set, type 'Y' or 'y' to clear error, anything else to abort")
            inp = input()
            if inp != "Y" and inp != "y":
                raise Exception("Error bit is set and user did not confirm to clear error, exiting")
            else:
                print("Clearing error bit and rerunning calibration")
                self.motor1.Send_Clear_Error()
                self.calibrate()
        self.open_gripper(blocking=True)
        print("Done calibrating.")
    
    def move_gripper(self, position=0, speed=0, current=300):
        """
        Position is from 0 -> 1 (0 is open, 1 is closed)
        Speed is from 0 -> 1 (higher is faster)
        Current is 0 to 1500 (mA). 
        These will be converted to raw values.

        Raw Position is from 0 -> 255 (0 is open, 255 is closed)
        Raw Speed is from 0 -> 255 (higher is faster)
        Current is 0 to 1500 (mA). 
        """

        assert 0 <= position <= 1, "Position must be between 0 and 1!"
        assert 0 <= speed <= 1, "Speed must be between 0 and 1!"
        assert 0 <= current <= 1500, "Current limit must be between 0 and 1000 mA for safety, if you want to go above 1000 mA change this function at your own risk"

        if current == 0 or speed == 0:
            print("WARNING: The gripper probably isn't moving because speed or current limit is 0")
        
        activate_send = 1
        action_send = 1
        estop_send = 0
        release_dir_send = 0

        # all position, speed, current must be rounded to an integer
        position = int(position * 255.)
        speed = int(speed * 255.)
        current = int(current)

        self.motor1.Send_gripper_data_pack(position, speed, current, activate_send, action_send, estop_send, release_dir_send)
    
    def open_gripper(self, speed=0.2, current=300, blocking=True):
        """
        Fully open gripper with reasonable defaults. Note that you need to sleep after this if you want to make sure it
        fully finishes opening
        if blocking=True, this function will not end until the gripper is fully open (or within a small margin.)
        """
        self.move_gripper(position=0, speed=speed, current=current)

        if blocking:
            gripper_pos = self.get_gripper_pos()
            while gripper_pos > self.OPEN_CLOSE_MARGIN / 255.:
                self.move_gripper(position=0, speed=speed, current=current)
                gripper_pos = self.get_gripper_pos()
                # idle since spamming commands seems to make speed unreliable
                pass

    
    def close_gripper(self, speed=0.2, current=300, blocking=True):
        """
        Fully close gripper with reasonable defaults. Note that you need to sleep after this if you want to make sure it
        fully finishes closing
        """
        self.move_gripper(position=1, speed=speed, current=current)
        if blocking:
            gripper_pos = self.get_gripper_pos()
            while gripper_pos < 1 - self.OPEN_CLOSE_MARGIN / 255.:
                self.move_gripper(position=1, speed=speed, current=current)
                # idle
                gripper_pos = self.get_gripper_pos()
                pass
    
    def get_gripper_data(self):
        new_message = 0
        self.motor1.Send_gripper_data_pack() # I think this asks the gripper to respond with the current gripper data        
        while new_message is not None: # Grab all new messages off the stack, there might be some stale ones which we should ignore
            new_message, n_UnpackedMessageID = self.communication1.receive_can_messages(timeout=0.005)
            if new_message is not None:
                good_msg = new_message
                good_id = n_UnpackedMessageID
        if new_message == 0:
            raise Error("Failed to get gripper position reading :(")
        self.motor1.UnpackData(good_msg,good_id) # Unpack the last data point
        self.most_recent_data = {
            "gripper_position": self.motor1.gripper_position,
            "gripper_current": self.motor1.gripper_current,
            "gripper_object_detection": self.motor1.gripper_object_detection, #0 = in motion, 1 = object detected while closing, 2 = detected while opening, 3 = static at position
            "gripper_calibrated": self.motor1.gripper_calibrated, # 1 = calibrated
            "gripper_activated": self.motor1.gripper_activated, # 1= activated
            "error_bit": good_id.error_bit, # 1 = error, 0 = no error
            "temperature_error": self.motor1.gripper_temperature_error,
            "timeout_error": self.motor1.gripper_timeout_error,
            "estop_error": self.motor1.gripper_estop_error
        }
        return self.most_recent_data

    
    def deactivate_gripper(self):
        self.motor1.Send_gripper_data_pack(0, 0, 0, 0, 0, 0, 0)

    def activate_gripper(self):
        self.motor1.Send_gripper_data_pack(0, 0, 0, 1, 0, 0, 0)
    
    def get_gripper_pos(self):
        """
        Return normalized (0-1) gripper position.
        """
        gripper_pos = self.get_gripper_data()["gripper_position"]
        return gripper_pos / 255.

    def close(self):
        # shutdown CAN bus
        self.deactivate_gripper()
        self.communication1.bus.shutdown()

if __name__ == "__main__":
    import time
    gripper = SSG48Gripper()
    gripper.open_gripper(speed=0.1, blocking=True)
    gripper.close_gripper(speed=0.05, blocking=True)
    # time.sleep(3)
    # gripper.open_gripper(speed=0.5, blocking=True)
    # time.sleep(3)
    gripper.close()
    # for i in range(1, 11):
    #     gripper.move_gripper(position=i*0.1, speed=0.3)
    #     print(gripper.get_gripper_pos())
    #     time.sleep(0.5)
    # time.sleep(2)
    # time.sleep(2)
    # print(gripper.get_gripper_data())
    # gripper.close_gripper()
    # time.sleep(2)
    # print(gripper.get_gripper_data())
    # print(gripper.get_gripper_pos())