示例#1
0
class SteerClaw:
    def __init__(self,
                 address,
                 dev_name,
                 baud_rate,
                 name,
                 kp1=0.7,
                 kp2=0.7,
                 ki1=0.2,
                 ki2=0.2,
                 kd1=30,
                 kd2=30,
                 int_windout1=20,
                 int_windout2=20,
                 qpps1=1,
                 qpps2=1,
                 deadzone1=3,
                 deadzone2=3,
                 kon1=0,
                 kon2=0,
                 sample_time=0.01,
                 last_time=0.00,
                 curren_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.targetAngleM1 = 0
        self.targetAngleM2 = 0
        self.kp1 = kp1
        self.kp2 = kp2
        self.ki1 = ki1
        self.ki2 = ki2
        self.kd1 = kd1
        self.kd2 = kd2
        self.qpps1 = qpps1
        self.qpps2 = qpps2
        self.deadzone1 = deadzone1
        self.deadzone2 = deadzone2
        self.int_windout1 = int_windout1
        self.int_windout2 = int_windout2
        self.kon1 = kon1
        self.kon2 = kon2
        self.sample_time = sample_time
        self.last_time = 0.00
        self.last_error1 = 0.00
        self.last_error2 = 0.00
        self.PTerm1 = 0.00
        self.ITerm1 = 0.00
        self.DTerm1 = 0.00
        self.PTerm2 = 0.00
        self.ITerm2 = 0.00
        self.DTerm2 = 0.00
        self.diff_ITerm1 = 0.00
        self.diff_ITerm2 = 0.00
        self.last_Ierr1 = 0.00
        self.last_Ierr2 = 0.00
        self.delta_error1 = 0.00
        self.delta_error2 = 0.00
        self.diff1 = 0.00
        self.diff2 = 0.00
        self.enc1Pos = 0.00
        self.enc2Pos = 0.00
        self.finalEnc1Val = 0.00
        self.finalEnc2Val = 0.00
        self.initialAngleM1 = 0
        self.initialAngleM2 = 0

    def update(self):
        global fin_ang
        self.current_time = time.time()
        delta_time = self.current_time - self.last_time
        #----------------------------------------------------
        #Roboclaw1
        if (delta_time >= self.sample_time):
            self.enc1Pos = self.initialAngleM1
            self.finalEnc1Val = fin_ang
            #rospy.loginfo(self.finalEnc1Val-self.enc1Pos)
            self.diff1 = self.finalEnc1Val - 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.kp1 * self.PTerm1) + (self.ki1 * self.ITerm1) +
                        (self.kd1 * self.DTerm1))

            if self.enc1Pos < (self.finalEnc1Val - self.deadzone1):
                velM1 = velM1 + self.kon1
                if (self.name == "forward"):
                    self.claw.BackwardM1(min(255, velM1))
                else:
                    self.claw.ForwardM1(min(255, velM1))

            elif self.enc1Pos > (self.finalEnc1Val + self.deadzone1):
                velM1 = velM1 - self.kon1
                if (self.name == "forward"):
                    self.claw.ForwardM1(min(255, -velM1))
                else:
                    self.claw.BackwardM1(min(255, -velM1))
            else:
                self.claw.ForwardM1(0)

            self.enc2Pos = self.initialAngleM2
            self.finalEnc2Val = fin_ang
            self.diff2 = self.finalEnc2Val - self.enc2Pos  #Error in 1
            self.delta_error2 = self.diff2 - self.last_error2
            self.PTerm2 = self.diff2  #Pterm
            self.ITerm2 += self.diff2 * delta_time

            if (self.last_Ierr2 != 0):
                self.diff_ITerm2 = self.ITerm2 - self.last_Ierr2

            if (self.ITerm2 < -self.int_windout2):
                self.ITerm2 = -self.int_windout2
            elif (self.ITerm2 > self.int_windout2):
                self.ITerm2 = self.int_windout2

            self.DTerm2 = 0.0
            if delta_time > 0:
                self.DTerm2 = self.delta_error2 / delta_time

            # Remember last time and last error for next calculation
            self.last_error2 = self.diff2
            self.last_Ierr2 = self.ITerm2
            velM2 = int((self.kp2 * self.PTerm2) + (self.ki2 * self.ITerm2) +
                        (self.kd2 * self.DTerm2))

            if self.enc2Pos < (self.finalEnc2Val - self.deadzone2):
                velM2 = velM2 + self.kon2
                if (self.name == "forward"):
                    self.claw.BackwardM2(min(255, velM2))
                else:
                    self.claw.BackwardM2(min(255, velM2))

            elif self.enc2Pos > (self.finalEnc2Val + self.deadzone2):
                velM2 = velM2 - self.kon2
                if (self.name == "forward"):
                    self.claw.ForwardM2(min(255, -velM2))
                else:
                    self.claw.ForwardM2(min(255, -velM2))

            else:
                self.claw.ForwardM2(0)
            print("Final Stopping Angle: " + str(fin_ang) + " Enc1Pos:" +
                  str(self.enc1Pos) + " Enc2Pos:" + str(self.enc2Pos))
示例#2
0
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)
示例#3
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)