class SteerClaw:
    def __init__(self,
                 address,
                 dev_name,
                 baud_rate,
                 name,
                 sample_time=0.1,
                 last_time=0.00,
                 current_time=0.00):
        self.ERRORS = {
            0x0000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "Normal"),
            0x0001:
            (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M1 over current"),
            0x0002:
            (diagnostic_msgs.msg.DiagnosticStatus.WARN, "M2 over current"),
            0x0004:
            (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Emergency Stop"),
            0x0008:
            (diagnostic_msgs.msg.DiagnosticStatus.ERROR, "Temperature1"),
            0x0010: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
                     "Temperature2"),
            0x0020: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
                     "Main batt voltage high"),
            0x0040: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
                     "Logic batt voltage high"),
            0x0080: (diagnostic_msgs.msg.DiagnosticStatus.ERROR,
                     "Logic batt voltage low"),
            0x0100: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
                     "M1 driver fault"),
            0x0200: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
                     "M2 driver fault"),
            0x0400: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
                     "Main batt voltage high"),
            0x0800: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
                     "Main batt voltage low"),
            0x1000: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
                     "Temperature1"),
            0x2000: (diagnostic_msgs.msg.DiagnosticStatus.WARN,
                     "Temperature2"),
            0x4000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M1 home"),
            0x8000: (diagnostic_msgs.msg.DiagnosticStatus.OK, "M2 home")
        }

        self.claw = RoboClaw(address, dev_name, baud_rate)
        self.name = name
        self.claw.ResetEncoders()
        self.sample_time = sample_time
        self.last_time = 0.00
        self.current_time = 0.00

    def setActuatorConstants(self):
        self.targetRpm1 = 0
        self.targetRpm2 = 0
        self.deltax = 0
        self.deltay = 0

    def pub_pot(self, pub):
        pot_val1 = self.claw.ReadEncM1()[1]
        pot_val2 = self.claw.ReadEncM2()[1]
        pot_val = Float64MultiArray(data=[pot_val1, pot_val2])
        pub.publish(pot_val)

    def pub_enc(self, pub):
        enc_val = self.claw.ReadEncM1()[1]
        pub.publish(enc_val)

    def pub_curr(self, pub):
        curr_val = self.claw.ReadCurrents()
        pub1.publish("Actuator Curr1 Val :" + str(curr_val[1]) +
                     "| Curr2 Val :" + str(curr_val[2]))

    def update_rpm(self):

        #velM1=int(deriv[0])
        velM1 = int(230 * self.deltax)
        if velM1 > 10:
            self.claw.ForwardM1(min(255, velM1))
        elif velM1 < -10:
            self.claw.BackwardM1(min(255, -velM1))
        else:
            self.claw.ForwardM1(0)

        #velM2=int(deriv[1])
        velM2 = int(230 * self.deltay)
        if velM2 > 10:
            self.claw.ForwardM2(min(255, velM2))
        elif velM2 < -10:
            self.claw.BackwardM2(min(255, -velM2))
        else:
            self.claw.ForwardM2(0)

    def setPIDconstants(self, Kp, Ki, Kd):
        self.kp = Kp
        self.ki = Ki
        self.kd = Kd
        self.mode = "lock"
        self.RPM_update = 0
        self.clmotor_speed = 0
        self.last_error1 = 0.00
        self.enc1Pos = 0.00
        self.last_error1 = 0.00
        self.PTerm1 = 0.00
        self.ITerm1 = 0.00
        self.DTerm1 = 0.00
        self.diff_ITerm1 = 0.00
        self.last_Ierr1 = 0.00
        self.delta_error1 = 0.00
        self.diff1 = 0.00
        self.enc1Pos = 0.00
        self.lockEnc1Val = 0.00
        self.initialAngleM1 = 0
        self.BR_update = 0
        self.int_windout1 = 10
        self.deadzone1 = 5

    def update_pid(self):
        self.current_time = time.time()
        delta_time = self.current_time - self.last_time
        #----------------------------------------------------
        #Roboclaw1
        if ((delta_time >= self.sample_time) and (self.mode == "lock")):
            self.enc1Pos = self.claw.ReadEncM1()[1]
            self.diff1 = self.lockEnc1Val - self.enc1Pos  #Error in 1

            self.delta_error1 = self.diff1 - self.last_error1
            self.PTerm1 = self.diff1  #Pterm
            self.ITerm1 += self.diff1 * delta_time

            if (self.last_Ierr1 != 0):
                self.diff_ITerm1 = self.ITerm1 - self.last_Ierr1

            if (self.ITerm1 < -self.int_windout1):
                self.ITerm1 = -self.int_windout1
            elif (self.ITerm1 > self.int_windout1):
                self.ITerm1 = self.int_windout1
            self.DTerm1 = self.delta_error1 / delta_time
            # Remember last time and last error for next calculation
            self.last_error1 = self.diff1
            self.last_Ierr1 = self.ITerm1

            #velM1 = int((self.kp*self.PTerm1) + (self.ki * self.ITerm1) + (self.kd * self.DTerm1))
            velM1 = 0
            if self.enc1Pos < (self.lockEnc1Val - self.deadzone1):
                self.claw.BackwardM1(min(255, velM1))
            elif self.enc1Pos > (self.lockEnc1Val + self.deadzone1):
                self.claw.ForwardM1(min(255, -velM1))
            else:
                self.claw.ForwardM1(0)

        if (self.mode == "roll"):
            #velM1=self.RPM_update
            velM1 = 0
            if velM1 > 0:
                self.claw.ForwardM1(min(255, velM1))
            elif velM1 < 0:
                self.claw.BackwardM1(min(255, -velM1))
            else:
                self.claw.ForwardM1(0)
            self.lockEnc1Val = self.claw.ReadEncM1()[1]
            print("Locking to position:", str(self.lockEnc1Val))

        velM2 = int(230 * self.BR_update)
        if velM2 > 10:
            self.claw.ForwardM2(min(255, velM2))
        elif velM2 < -10:
            self.claw.BackwardM2(min(255, -velM2))
        else:
            self.claw.ForwardM2(0)
Beispiel #2
0
class ArmClaw:

    def __init__(self, address, dev_name, baud_rate, reset_encoders=False):
        self.claw = RoboClaw(address, dev_name, baud_rate)
        if(reset_encoders):
            self.claw.ResetEncoders()
        
    def setActuatorConstants(self):
        #When these variables are set to +/- 1 M1, M2 move fwd bkwd
        self.actuatorclawM1=0
        self.actuatorclawM2=0

    def setGripperConstants(self):
        #When grip roll is set to 1, we get ACW rotn o/w CW. Fing_close=1 means fingers will be closed
    	self.grip_roll=0
    	self.fing_close=0

    def pub_pot(self, pub):
        #Read absolute encoder values and publish
        pot_val1 = self.claw.ReadEncM1()[1]
        pot_val2 = self.claw.ReadEncM2()[1]
        pot_val=Float64MultiArray(data=[pot_val1,pot_val2])
        pub.publish(pot_val)

    def pub_enc(self, pub):
        #Read quadrature encoder values
        enc_val = self.claw.ReadEncM2()[1]
        pub.publish(enc_val)

    # Update Actuator motions
    def update_actuators(self):
        #Make actuatorclawM1 move
        velM1=int(230*self.actuatorclawM1)
        if velM1>0:
            self.claw.ForwardM1(min(255, velM1))
        elif velM1<0:
            self.claw.BackwardM1(min(255, -velM1))
        else:
            self.claw.ForwardM1(0)

        #Make acctuatorclawM2 move
        velM2=int(230*self.actuatorclawM2)
        if velM2>10:
            self.claw.ForwardM2(min(255, velM2))
        elif velM2<-10:
            self.claw.BackwardM2(min(255, -velM2))
        else:
            self.claw.ForwardM2(0)

    def update_gripper(self):
        # Close/Open the fingers
        velM1=int(230*self.fing_close)
        if velM1>10:
            self.claw.ForwardM1(min(255, velM1))
        elif velM1<-10:
            self.claw.BackwardM1(min(255, -velM1))
        else:
            self.claw.ForwardM1(0)
        
        #Make the gripper roll
        velM2=int(230*self.grip_roll)
        if velM2>10:
            self.claw.ForwardM2(min(255, velM2))
        elif velM2<-10:
            self.claw.BackwardM2(min(255, -velM2))
        else:
            self.claw.ForwardM2(0)