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