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)
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)