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