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)
# speed = CMF.readAcSpeed(rc)
# pw = CMF.readSafeSpeed(rc)
# # Stop Motor
# CMF.stopMotor(rc)
# # Turn Off Brake
# brake.setTorque(Mc, 0)
# print('brake command sent')
# brake.close(Nb, Mc)
# print('brake closed')
# print(speed)
# commented out full test for 0 25 50 75 100 50 0 50 100 0 100 0
torqueList = [0, 25, 50, 75, 100, 50, 0]
speedList = []
for torque in torqueList:
guess = motorModel(40, torque)
CMF.setMotorSpeed(rc, guess)
brake.setTorque(Mc, torque * 10)
# Wait to Speed Up
time.sleep(2)
# Read Speed
speedList.append(CMF.readAcSpeed(rc))
# Stop Motor
CMF.stopMotor(rc)
brake.setTorque(Mc, 0)
print('brake command sent')
brake.close(Nb, Mc)
print('brake closed')
print(speedList)
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)
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 = 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
import brake
timeLength = 3.0 # seconds
pts = 100
brakeStrength = [0, 25, 50, 75, 100]
motorSpeed = range(20, 90, 10)
data = np.full([len(motorSpeed), len(brakeStrength)], np.nan)
rc = RoboClaw('COM10', 0x80)
Nb, Mc = brake.initNebula()
# Nb = brake.initNebula()
time.sleep(1)
for speed in range(len(motorSpeed)):
CMF.setMotorSpeed(rc, motorSpeed[speed])
for strength in range(len(brakeStrength)):
brake.setTorque(Mc, 10 * brakeStrength[strength])
# for strength in range(len(brakeStrength)):
# brake.setTorque(Mc, 10* brakeStrength[strength])
# for speed in range(len(motorSpeed)):
# # print(motorSpeed[speed])
# CMF.setMotorSpeed(rc, motorSpeed[speed])
# wait until Steady state confirmed
# for t in range(pts):
# figure out real arrangement, maybe a class
time.sleep(2)
avg, v = CMF.readAvgCurrent(rc, timeLength, pts)
data[speed, strength] = avg
# time.sleep(timeLength / pts) #built into function
# Turn off the danger, it's time for science