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
import time
from roboclaw import RoboClaw
import CalibrationMotorFunctions as CMF
import brake
rc = RoboClaw('COM7', 0x80)
Nb= brake.initNebula()
time.sleep(1)
#roboclaw1.drive_to_position_raw(motor=1, accel=0, speed=0, deccel=0, position=25, buffer=1)
#roboclaw1.stop_all()
# print(roboclaw1.read_max_speed(1))
# print(roboclaw1.read_position(1))
brake.setTorque(Nb, 50)
CMF.setMotorSpeed(rc,10)
time.sleep(.6)
print(CMF.readAvgCurrent(rc,10))
time.sleep(3)
CMF.stopMotor(rc)
brake.setTorque(Nb,0)
def motorModel(speed, torque):
# Warning on tested for speed 5
# pwm = 0.0004 * (torque ** 2) + 0.0965 * torque + 1.666 + speed
pwm = 0.0011 * (torque**2) + 0.0318 * torque + speed
# B = 1.16
# M = .1404
# pwm = speed + B + M * torque
return pwm
# Initialize everything
print('Initializing')
tc = Ard_T('COM3', 1)
rc = RoboClaw('COM11', 0x80)
Nb, Mc = brake.initNebula()
# desiredSpeed = 40
# guess = 52
# # Set Brake Torque
# brake.setTorque(Mc, 1000)
# # Slight Delay
# time.sleep(.25)
# # Turn On Motor Guess of Appropriate Speed
# CMF.setMotorSpeed(rc, guess)
# # Wait to Speed Up
# time.sleep(2)
# # Read Speed
# speed = CMF.readAcSpeed(rc)
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)
self.dir2 = -1
elif (inp.data[6] < -10):
self.speed = inp.data[6] * 25 * inp.data[6] / 800
self.dir1 = -1
self.dir2 = 1
if __name__ == "__main__":
rospy.init_node("Differential node")
rospy.loginfo("Starting differential node")
r_time = rospy.Rate(1)
for i in range(20):
try:
roboclaw2 = RoboClaw(0x81, "/dev/roboclaw2", 9600)
except SerialException:
rospy.logwarn("Could not connect to RoboClaw2, retrying...")
r_time.sleep()
rospy.loginfo("Connected to RoboClaw2")
for i in range(20):
try:
roboclaw1 = RoboClaw(0x80, "/dev/roboclaw1", 9600)
except SerialException:
rospy.logwarn("Could not connect to RoboClaw1, retrying...")
r_time.sleep()
rospy.loginfo("Connected to RoboClaw1")
diffClaw = DifferentialClaw(roboclaw1, roboclaw2)
from roboclaw import RoboClaw
import time
roboclaw1 = RoboClaw(port='/dev/ttyACM0', address=0x80)
# Read the roboclaw manual to understand the trajectory parameters
roboclaw1.drive_to_position_raw(motor=1,
accel=0,
speed=0,
deccel=0,
position=25,
buffer=1)
while True:
print(roboclaw1.read_position(1))
time.sleep(0.5)
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 collectBrakeData(trials, currdir, fname, timeLength=3, pts=150, atrials=0):
""" For Specified number trials
collect information in data
data = trialsx3 array
of columns input voltage, current, label
"""
# rc = RoboClaw('COM10', 0x80)
# Nb = brake.initNebula()
# # imports and set ups
# data = np.zeros(trials, 3)
# motorSpeedList = [50]
# maxTorque = 100.0
# data[0, 2] = 0
# brake.setTorque(Nb, strength)
# CMF.setMotorSpeed(motorSpeedList[0])
# noLoadCurrent = CMF.readAvgCurrent(rc, timeLength, pts)
# for t in range(trials):
# # Input random voltage for torque
# strength = np.random.randint(0, 1001)
# data[t, 0] = strength
# brake.setTorque(Nb, strength)
# # Set motor Speed based on estimate to reduce variance
# CMF.setMotorSpeed(motorSpeedList[np.floor(
# brake / maxTorque * len(motorSpeedList))])
# # Read value current I from controller
# I, v = CMF.readAvgCurrent(rc, timeLength, pts)
# # Do Interpolation to calculate Torque
# data[t, 1] = np.nan # TODO math
# # Make Array of Input voltage and corresponding torque
# # Label Data based on if previous value was
# if t > 0:
# data[t, 2] = data[t - 1, 1]
# # less than (0) or greater than (1) new value
# # First value is always zero
if type(trials) is list:
brakeStrength = np.asarray(trials)
trials = len(trials)
elif isinstance(trials, np.ndarray):
brakeStrength = trials
trials = len(trials)
else:
brakeStrength = np.random.random_integers(0, 1000, (trials, )) / 10.0
if atrials > 0:
step = 60
cutoff = 100
asymSteps = np.zeros(atrials)
for i in range(1, atrials // 2, 2):
asymSteps[i] = min(
1000 - cutoff,
asymSteps[i - 1] + 2 * np.random.random_integers(0, step))
asymSteps[i + 1] = max(
0, asymSteps[i] - 1 * np.random.random_integers(0, step))
asymSteps[atrials // 2] = 1000
for i in range(atrials // 2 + 1, atrials - 1, 2):
asymSteps[i] = max(
0 + cutoff,
asymSteps[i - 1] - 2 * np.random.random_integers(0, step))
asymSteps[i + 1] = min(
1000, asymSteps[i] + 1 * np.random.random_integers(0, step))
asymSteps[-1] = max(
0 + cutoff, asymSteps[-2] - 2 * np.random.random_integers(0, step))
brakeStrength = np.append(brakeStrength, asymSteps / 10)
print(np.shape(brakeStrength))
plt.plot(brakeStrength)
plt.show()
currentScale = 1000 / 100
pause = 0
motorSpeed = 5 # [20, 20, 20, 20, 20]
fullTime = timeLength * len(brakeStrength)
rc = RoboClaw('COM11', 0x80)
Nb, Mc = brake.initNebula()
dt = 1.0 / pts
P = 7
D = 15
I = .1
# P = 0
# D = 0
# I = 0
# Create List of Commands
intError = 0
lastError = 0
data = np.full([int(fullTime * pts), 5], np.nan)
if pause <= 0:
CMF.setMotorSpeed(rc, motorSpeed)
time.sleep(1)
for point in range(0, len(brakeStrength)):
# initialize Data
# Main Loop
if pause > 0:
intError = 0
lastError = 0
CMF.setMotorSpeed(rc, motorSpeed)
time.sleep(1)
stepTime = 0.0
currTime = stepTime + timeLength * point
start = time.time()
while stepTime < timeLength:
setSpeed = motorSpeed # [int(np.floor(currTime / timeLength))]
acSpeed = CMF.readAcSpeed(rc)
error = setSpeed - acSpeed
derror = error - lastError
intError += error
command = P * error + D * derror + I * intError
CMF.setMotorSpeed(rc, command)
lastError = error
brakeTorque = int(
round(currentScale *
brakeStrength[int(np.floor(currTime / timeLength))]))
brake.setTorque(Mc, brakeTorque)
# Record Data here
# TimeStamp
data[int(np.floor(currTime / dt)), 0] = currTime
# Brake Command
data[int(np.floor(currTime / dt)),
1] = brakeStrength[int(np.floor(currTime / timeLength))]
# Actual Brake Reading
data[int(np.floor(currTime / dt)),
2] = brake.readCurrent(Mc) / currentScale
# Motor Current
data[int(np.floor(currTime / dt)), 3] = CMF.readInCurrent(rc)
# Motor Speed
data[int(np.floor(currTime / dt)), 4] = acSpeed
# data[int(np.floor(currTime / dt)), 3] = setSpeed
loopTime = time.time() - stepTime - start
if loopTime < dt:
time.sleep(dt - loopTime)
stepTime = time.time() - start
currTime = stepTime + timeLength * point
if pause > 0:
CMF.stopMotor(rc)
time.sleep(pause * brakeStrength[point] / 100.0)
CMF.stopMotor(rc)
np.savetxt(
currdir + fname + '.csv',
data,
fmt='%.2f',
delimiter=',',
newline='\n',
header=
('Time, setPoint, Actual Brake Current, MotorCurrent, MotorSpeed, trials: %d , atrials: %d , pts: %d , timeLength: %0.2f'
% (trials, atrials, pts, timeLength)),
footer='',
comments='# ')
np.savetxt(currdir + 'BrakeCommands' + fname + '.csv',
brakeStrength,
fmt='%.1f',
delimiter=',',
newline='\n',
header='',
footer='',
comments='# ')
brake.setTorque(Mc, 0)
print('brake command sent')
brake.close(Nb, Mc)
print('brake closed')
return (data, brakeStrength)
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))
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()
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)
def warmup(runtime=1200):
rc = RoboClaw('COM11', 0x80)
CMF.setMotorSpeed(rc, 5)
time.sleep(runtime)
CMF.stopMotor(rc)
print('Warmup Script Complete')
import brake
import time
from roboclaw import RoboClaw
import CalibrationMotorFunctions as CMF
print("starting")
# Nb, Mc = brake.initNebula()
# print((Nb,Mc))
# for i in range(1):
# brake.setTorque(Mc, 0)
# time.sleep(.2)
# brake.setTorque(Mc, 1000)
# time.sleep(5)
# brake.setTorque(Mc, 0)
# print('Stress Test Passed')
# brake.close(Nb, Mc)
# print('Motor Closed without Fault')
# print('All Tests Passed')
# rc = RoboClaw('COM7', 0x80)
# # Nb = brake.initNebula()
# time.sleep(1)
# CMF.setMotorSpeed(rc,90)
# time.sleep(2)
# CMF.stopMotor(rc)
rc = RoboClaw('COM10', 0x80)
CMF.setMotorSpeed(rc, 0)
CMF.setMotorSpeed(rc, 20)
time.sleep(3)
CMF.stopMotor(rc)
