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)