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)
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 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)
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)
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 __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 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)
from roboclaw import RoboClaw
import time
roboclaw1 = RoboClaw('/dev/ttyACM0', 0x80)
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)