def init_rc(): global rc global rc_address # Initialise the roboclaw motorcontroller print("Initialising roboclaw driver...") from roboclaw import Roboclaw rc_address = 0x80 rc = Roboclaw("/dev/roboclaw", 115200) rc.Open() # Get roboclaw version to test if is attached version = rc.ReadVersion(rc_address) if version[0] is False: print("Roboclaw get version failed") sys.exit() else: print(repr(version[1])) # Set motor controller variables to those required by K9 rc.SetM1VelocityPID(rc_address, M1_P, M1_I, M1_D, M1_QPPS) rc.SetM2VelocityPID(rc_address, M2_P, M2_I, M2_D, M2_QPPS) rc.SetMainVoltages(rc_address,232,290) # 23.2V min, 29V max rc.SetPinFunctions(rc_address,2,0,0) # Zero the motor encoders rc.ResetEncoders(rc_address) # Print Motor PID Settings m1pid = rc.ReadM1VelocityPID(rc_address) m2pid = rc.ReadM2VelocityPID(rc_address) print("M1 P: " + str(m1pid[1]) + ", I:" + str(m1pid[2]) + ", D:" + str(m1pid[3])) print("M2 P: " + str(m2pid[1]) + ", I:" + str(m2pid[2]) + ", D:" + str(m2pid[3])) # Print Min and Max Main Battery Settings minmaxv = rc.ReadMinMaxMainVoltages(rc_address) # get min max volts print ("Min Main Battery: " + str(int(minmaxv[1])/10) + "V") print ("Max Main Battery: " + str(int(minmaxv[2])/10) + "V") # Print S3, S4 and S5 Modes S3mode=['Default','E-Stop (latching)','E-Stop','Voltage Clamp','Undefined'] S4mode=['Disabled','E-Stop (latching)','E-Stop','Voltage Clamp','M1 Home'] S5mode=['Disabled','E-Stop (latching)','E-Stop','Voltage Clamp','M2 Home'] pinfunc = rc.ReadPinFunctions(rc_address) print ("S3 pin: " + S3mode[pinfunc[1]]) print ("S4 pin: " + S4mode[pinfunc[2]]) print ("S5 pin: " + S5mode[pinfunc[3]]) print("Roboclaw motor controller initialised...")
class Agitator(object): '''Class for controlling the EXPRES fiber agitator Inputs ------ comport : str The hardware COM Port for the Roboclaw motor controller Public Methods -------------- start(exp_time, timeout, rot1, rot2): Threaded agitation start_agitation(exp_time, rot1, rot2): Unthreaded agitation stop(): Stop either threaded or unthreaded agitation stop_agitation(): Hard-stop agitation but will not close thread ''' def __init__(self, comport=__DEFAULT_PORT__): self._rc = Roboclaw(comport=comport, rate=__DEFAULT_BAUD_RATE__, addr=__DEFAULT_ADDR__, timeout=__DEFAULT_TIMEOUT__, retries=__DEFAULT_RETRIES__, inter_byte_timeout=__DEFAULT_INTER_BYTE_TIMEOUT__) # Create a logger for the agitator self.logger = logging.getLogger('expres_agitator') self.logger.setLevel(logging.DEBUG) # Create file handler to log all messages try: fh = logging.handlers.TimedRotatingFileHandler( 'C:/Users/admin/agitator_logs/agitator.log', when='D', interval=1, utc=True, backupCount=5) except FileNotFoundError: fh = logging.handlers.TimedRotatingFileHandler('agitator.log', when='D', interval=1, utc=True, backupCount=5) fh.setLevel(logging.DEBUG) # Create console handler to log info messages ch = logging.StreamHandler() ch.setLevel(logging.INFO) # create formatter and add it to the handlers formatter = logging.Formatter( '%(asctime)s - %(name)s - %(levelname)s - %(message)s') fh.setFormatter(formatter) ch.setFormatter(formatter) # add the handlers to the logger self.logger.addHandler(fh) self.logger.addHandler(ch) self.thread = None # In case stop() is called before a thread is created self.stop_event = Event() # Used for stopping threads self.stop_agitation() # Just to make sure def __del__(self): ''' When the object is deleted, make sure all threads are closed and agitator is stopped. Unfortunately, these actions cannot be logged because the logger closes by the time __del__ is called... ''' self.stop(verbose=False) self.stop_agitation(verbose=False) def threaded_agitation(self, exp_time, timeout, **kwargs): '''Threadable function allowing stop event''' self.logger.info( 'Starting agitator thread for {}s exposure with {}s timeout'. format(exp_time, timeout)) self.start_agitation(exp_time, **kwargs) t = 0 start_time = time.time() while not self.stop_event.is_set() and t < timeout: time.sleep(1) t = time.time() - start_time self.logger.info('{}/{}s for {}s exposure. I1: {}, I2: {}'.format( round(t, 1), timeout, exp_time, self.current1, self.current2)) self.stop_agitation() self.stop_event.clear() # Allow for future agitation events def start(self, exp_time=60.0, timeout=None, **kwargs): ''' Start a thread that starts agitation and stops if a stop event is called or if the timeout is reached ''' self.stop() # To close any previously opened threads if timeout is None: # Allow for some overlap time timeout = exp_time + 10.0 self.thread = Thread(target=self.threaded_agitation, args=(exp_time, timeout), kwargs=kwargs) self.thread.start() def stop(self, verbose=True): '''Stop the agitation thread if it is running''' if self.thread is not None and self.thread.is_alive(): while self.voltage1 > 0 or self.voltage2 > 0: if verbose: self.logger.info('Attempting to stop threaded agitation') self.stop_event.set() self.thread.join(2) if self.voltage1 > 0 or self.voltage2 > 0: # As a backup in case something went wrong if verbose: self.logger.error( 'Something went wrong when trying to stop threaded agitation. Forcing agitator to stop.' ) self.stop_agitation() def start_agitation(self, exp_time=60.0, rot=None): '''Set the motor voltages for the given number of rotations in exp_time''' if exp_time <= 0: self.logger.warning( 'Non-positive exposure time given to agitator object') self.stop_agitation() return if rot is None: rot = 0.5 * exp_time if rot <= 0: self.logger.warning( 'Non-positive rotation number given to agitator object') self.stop_agitation() return freq1 = rot / exp_time freq2 = 0.9 * rot / exp_time self._freq = freq1 self.logger.info('Starting agitation at approximately {} Hz'.format( self._freq)) self.set_voltage1(Motor1.calc_voltage(self.battery_voltage, freq1)) self.set_voltage2(Motor2.calc_voltage(self.battery_voltage, freq2)) def stop_agitation(self, verbose=True): '''Set both motor voltages to 0''' if verbose: self.logger.info('Stopping agitation') self.set_voltage(0) self._freq = 0 def set_voltage(self, voltage): '''Set both motor voltages to the given voltage''' self.set_voltage1(voltage) self.set_voltage2(voltage) # Getter for the frequency def get_freq(self): self.logger.info('Requesting frequency') return self._freq # Getters and setters for the motor voltages def get_voltage1(self): return self._voltage1 def set_voltage1(self, voltage): battery_voltage = self.battery_voltage if abs(voltage) > battery_voltage: voltage = np.sign(voltage) * battery_voltage if voltage >= 0: self._rc.ForwardM1(int(voltage / battery_voltage * 127)) else: self._rc.BackwardM1(int(-voltage / battery_voltage * 127)) self._voltage1 = voltage voltage1 = property(get_voltage1, set_voltage1) def get_voltage2(self): return self._voltage2 def set_voltage2(self, voltage): battery_voltage = self.battery_voltage if abs(voltage) > battery_voltage: voltage = np.sign(voltage) * battery_voltage if voltage >= 0: self._rc.ForwardM2(int(voltage / battery_voltage * 127)) else: self._rc.BackwardM2(int(-voltage / battery_voltage * 127)) self._voltage2 = voltage voltage2 = property(get_voltage2, set_voltage2) def get_max_voltage(self): return self._rc.ReadMinMaxMainVoltages()[2] / 10 def set_max_voltage(self, voltage): self._rc.SetMainVoltages(int(self.min_voltage * 10), int(voltage * 10)) max_voltage = property(get_max_voltage, set_max_voltage) def get_min_voltage(self): return self._rc.ReadMinMaxMainVoltages()[1] / 10 def set_min_voltage(self, voltage): self._rc.SetMainVoltages(int(voltage * 10), int(self.max_voltage * 10)) min_voltage = property(get_min_voltage, set_min_voltage) # Getter for the motor controller power source voltage def get_battery_voltage(self): return self._rc.ReadMainBatteryVoltage()[1] / 10 battery_voltage = property(get_battery_voltage) # Getters and setters for the motor currents def get_current1(self): return self._rc.ReadCurrents()[1] / 100 current1 = property(get_current1) def get_current2(self): return self._rc.ReadCurrents()[2] / 100 current2 = property(get_current2) def get_max_current1(self): return self._rc.ReadM1MaxCurrent()[1] / 100 def set_max_current1(self, current): self._rc.SetM1MaxCurrent(current) max_current1 = property(get_max_current1, set_max_current1) def get_max_current2(self): return self._rc.ReadM2MaxCurrent()[1] / 100 def set_max_current2(self, current): self._rc.SetM2MaxCurrent(current) max_current2 = property(get_max_current2, set_max_current2)
class SerialDriver(object): ''' Class for serial UART interface to the RoboClaw Motor Controllers ''' def __init__(self): rospy.loginfo("Initilizing the motor controllers..") self.e_stop = 1 self.reg_enabled = 0 self.temp = 0 self.error = 0 self.voltage = 0 self.currents = [0,0] self._thread_lock = False self.prev_enc_ts = None self.prev_tick = [None, None] self.start_time = datetime.now() self.left_currents = [] self.right_currents = [] self.max_left_integrator = 0 self.max_right_integrator = 0 self.left_integrator = 0 self.right_integrator = 0 self.motor_lockout = 0 self._cmd_buf_flag = 0 self._l_vel_cmd_buf = 0 self._r_vel_cmd_buf = 0 self.battery_percent = 0 self.shutdown_warning = False self.shutdown_flag = False self.rate = rospy.get_param("/puffer/rate") self.delta_t = 2.0/self.rate self.rc = Roboclaw( rospy.get_param("/motor_controllers/serial_device_name"), rospy.get_param("/motor_controllers/baud_rate") ) self.rc.Open() self._set_operating_params() self._get_version() self.set_estop(0) #Clears E-stop pin self.enable_12v_reg(1) #Disables 12V Regulator self.kill_motors() #Start the motors not moving #Private Methods def _get_version(self): ''' Version check for communication verification to the motor controllers returns ther version number if sucessful, and 0 if not ''' version = self.rc.ReadVersion(self.address) if version != 0: rospy.loginfo("[Motor __init__ ] Sucessfully connected to all Motor Controllers!") rospy.loginfo( version ) rospy.set_param("/motor_controllers/firmware_version",version) else: raise Exception("Unable to establish connection to Motor controllers") return version def _set_operating_params(self): ''' Sets all the operating parameters for control of PUFFER, Pinouts, and safety parameters. returns None ''' #GPIO settings for E-stop and Regulator Enable pins self.e_stop_pin = rospy.get_param("/gpio_pins/e_stop",1) self.reg_en_pin = rospy.get_param("/gpio_pins/reg_en",1) try: GPIO.setmode(GPIO.BCM) GPIO.setwarnings(False) GPIO.setup(self.e_stop_pin, GPIO.OUT) GPIO.setup(self.reg_en_pin, GPIO.OUT) except: pass #Threadlock used for serial comm to RoboClaw self.thread_timeout = rospy.get_param("/threadlock/timeout") #Motor operating parameters self.wheel_d = rospy.get_param("/wheels/diameter") self.enc_cpr = rospy.get_param("/wheels/encoder/cts_per_rev") factor = rospy.get_param("/wheels/encoder/factor") stage_1 = rospy.get_param("/wheels/gearbox/stage1") stage_2 = rospy.get_param("/wheels/gearbox/stage2") stage_3 = rospy.get_param("/wheels/gearbox/stage3") self.accel_const = rospy.get_param("/puffer/accel_const") self.max_vel_per_s = rospy.get_param("/puffer/max_vel_per_s") self.tick_per_rev = int(self.enc_cpr * factor * stage_1 * stage_2 * stage_3) rospy.loginfo(self.tick_per_rev) rospy.set_param("tick_per_rev", self.tick_per_rev) self.max_cts_per_s = int((self.max_vel_per_s * self.tick_per_rev)/(math.pi * self.wheel_d)) self.max_accel = int(self.max_cts_per_s * self.accel_const) rospy.set_param("max_cts_per_s", self.max_cts_per_s) rospy.set_param("max_accel", self.max_accel) self.address = rospy.get_param("/motor_controllers/address", 0x80) self.rc.SetMainVoltages(self.address,int(rospy.get_param("/motor_controllers/battery/main/low", 12.0) * 10),int(rospy.get_param("motor_controllers/battery/main/high", 18.0 ) * 10)) self.rc.SetLogicVoltages(self.address,int(rospy.get_param("/motor_controllers/battery/logic/low") * 10),int(rospy.get_param("motor_controllers/battery/logic/high") * 10)) self.max_current = rospy.get_param("/motor_controllers/current/max_amps") self.motor_lockout_time = rospy.get_param("/puffer/motor_lockout_time") m1p = rospy.get_param("/motor_controllers/m1/p") m1i = rospy.get_param("/motor_controllers/m1/i") m1d = rospy.get_param("/motor_controllers/m1/d") m1qpps = rospy.get_param("/motor_controllers/m1/qpps") m2p = rospy.get_param("/motor_controllers/m2/p") m2i = rospy.get_param("/motor_controllers/m2/i") m2d = rospy.get_param("/motor_controllers/m2/d") m2qpps = rospy.get_param("/motor_controllers/m2/qpps") self.battery_max_time = rospy.get_param("/battery/max_time") self.battery_max_volts = rospy.get_param("/battery/max_volts") self.battery_coef_a = rospy.get_param("/battery/a") self.battery_coef_b = rospy.get_param("/battery/b") self.battery_coef_c = rospy.get_param("/battery/c") self.battery_warning = rospy.get_param("/battery/warning_percent") self.battery_shutdown = rospy.get_param("/battery/shutdown_percent") self.rc.SetM1VelocityPID(self.address,m1p, m1i, m1d, m1qpps) self.rc.SetM2VelocityPID(self.address,m2p, m2i, m2d, m2qpps) self.rc.WriteNVM(self.address) time.sleep(0.001) self.rc.ReadNVM(self.address) def _lock_thread(self,lock): ''' Checks the thread lock and then grabs it when it frees up no return value ''' if (lock): start = time.time() while self._thread_lock: #rospy.loginfo("in threadlock") if time.time() - start > self.thread_timeout: raise Exception("Thread lock timeout") time.sleep(0.001) self._thread_lock = True else: self._thread_lock = False def _get_Temp(self): ''' Gets the temperature of the motor controllers return: list [2] (int): Temperature values * 10 degrees C ''' self._lock_thread(1) self.temp = self.rc.ReadTemp(self.address)[1] self._lock_thread(0) self.temp = int(self.temp*100)/1000.0 return self.temp def _get_Voltage(self): ''' Gets the voltage of the motor controllers return: voltage (int) : Voltage values * 10 volts ''' self._lock_thread(1) v = self.rc.ReadMainBatteryVoltage(self.address)[1] v = int(v*100)/1000.0 if v != 0: self.voltage = v + 0.4 #accounts for the voltage drop in the diode self._lock_thread(0) return v def _get_Currents(self): ''' Gets the current of the motor controllers return: list [2] (int): Current values * 100 Amps ''' self._lock_thread(1) cur = self.rc.ReadCurrents(self.address) self._lock_thread(0) r_current = int(cur[1])/100.0 l_current = int(cur[2])/100.0 self.currents = [l_current,r_current] self.left_currents.insert(0, l_current) self.right_currents.insert(0, r_current) if (len(self.left_currents) > self.rate/2): del self.left_currents[-1] del self.right_currents[-1] left_power = 0 right_power = 0 for i in range(len(self.left_currents)): left_power += math.pow(self.left_currents[i],2) * self.delta_t right_power += math.pow(self.right_currents[i],2) * self.delta_t if (left_power >= math.pow(self.max_current,2) or right_power >= math.pow(self.max_current,2)): rospy.loginfo("Motor power exceeded max allowed! Disabling motors for %d seconds" %(self.motor_lockout_time)) self.motor_lockout = 1 self.set_estop(1) self.lockout_timestamp = time.time() if (self.motor_lockout and (time.time() - self.lockout_timestamp >= self.motor_lockout_time)): rospy.loginfo("Re-enabling the motors from timeout lock") self.motor_lockout = 0 self.set_estop(0) self.left_integrator = left_power self.right_integrator = right_power self.max_left_integrator = max(self.max_left_integrator, self.left_integrator) self.max_right_integrator = max(self.max_right_integrator, self.right_integrator) #print self.left_integrator, self.right_integrator return self.currents def _get_Errors(self): ''' Gets the error status of the motor controllers return: error (int): Error code for motor controller ''' self._lock_thread(1) self.error = self.rc.ReadError(self.address)[1] self._lock_thread(0) return self.error def _get_Encs(self): ''' Gets the encoder values of the motor controllers return: list [2] (int): Speed of motors in radians/s, computed at each delta T ''' self._lock_thread(1) r_enc = self.rc.ReadEncM1(self.address)[1] l_enc = self.rc.ReadEncM2(self.address)[1] dt = datetime.now() self._lock_thread(0) if self.prev_tick == [None, None]: l_vel_rad_s, r_vel_rad_s = 0,0 self.timestamp = 0 else: delta_t = ((dt-self.prev_enc_ts).microseconds)/1000000.0 self.timestamp = int(self.timestamp + (delta_t * 100000)) l_vel = (l_enc - self.prev_tick[0])/(delta_t) r_vel = (r_enc - self.prev_tick[1])/(delta_t) l_vel_rad_s = l_vel * (2 * math.pi/self.tick_per_rev) r_vel_rad_s = r_vel * (2 * math.pi/self.tick_per_rev) l_vel_rad_s = int(l_vel_rad_s * 1000)/1000.0 r_vel_rad_s = int(r_vel_rad_s * 1000)/1000.0 self.prev_enc_ts = dt self.prev_tick = [l_enc, r_enc] self.enc = [l_vel_rad_s, r_vel_rad_s] return l_vel_rad_s, r_vel_rad_s def _send_motor_cmds(self): l_vel = self._l_vel_cmd_buf r_vel = self._r_vel_cmd_buf l_vel *= (self.tick_per_rev/(2*math.pi)) r_vel *= (self.tick_per_rev/(2*math.pi)) l_vel = max(min(l_vel,self.max_cts_per_s), -self.max_cts_per_s) r_vel = max(min(r_vel,self.max_cts_per_s), -self.max_cts_per_s) if (( abs(l_vel) <= self.max_cts_per_s) and (abs(r_vel) <= self.max_cts_per_s)): if not self.motor_lockout: self._lock_thread(1) self.rc.SpeedAccelM1(self.address, self.max_accel, int(r_vel)) self.rc.SpeedAccelM2(self.address, self.max_accel, int(l_vel)) self._lock_thread(0) else: rospy.loginfo( "values not in accepted range" ) self.send_motor_cmds(0,0) # Public Methods def set_estop(self,val): ''' Sets the E-stop pin to stop Motor control movement until cleared Parameters: val (int): 0 - Clears E-stop 1 - Sets E-stop, disabling motor movement no return value ''' if (val != self.e_stop): if val: rospy.loginfo( "Enabling the E-stop") GPIO.output(self.e_stop_pin, 1) self.e_stop = 1 else: rospy.loginfo( "Clearing the E-stop") GPIO.output(self.e_stop_pin, 0) self.e_stop = 0 def battery_state_esimator(self): x = self.voltage y = math.pow(x,2) * self.battery_coef_a + self.battery_coef_b * x + self.battery_coef_c self.battery_percent = (100 * y/float(self.battery_max_time)) if (self.battery_percent <= self.battery_warning): self.shutdown_warning = True else: self.shutdown_warning = False if (self.battery_percent <= self.battery_shutdown): self.shutdown_flag = True self.set_estop(1) def update_cmd_buf(self, l_vel, r_vel): ''' Updates the command buffers and sets the flag that new command has been received Parameters: l_vel (int): [-0.833, 0.833] units of [rad/s] r_vel (int): [-0.833, 0.833] units of [rad/s] no return value ''' self._l_vel_cmd_buf = l_vel self._r_vel_cmd_buf = r_vel self._cmd_buf_flag = 1 def kill_motors(self): ''' Stops all motors on the assembly ''' self._lock_thread(1) self.rc.ForwardM1(self.address, 0) self.rc.ForwardM2(self.address, 0) self._lock_thread(0) def loop(self, counter): ''' Gets the data from the motor controllers to populate as class variables all data function classes are private because of threadlocks Downsample non-critical values to keep serial comm line free Parameters: counter (int): no return value ''' if (self._cmd_buf_flag): self._send_motor_cmds() self._cmd_buf_flag = 0 self._get_Encs() if not counter % 2: self._get_Currents() if not counter % 5: self._get_Errors() if not counter % 20: self._get_Temp() self._get_Voltage() self.battery_state_esimator() def enable_12v_reg(self, en): ''' Turns on/off the 12V regulator GPIO pin Parameters: en (int): 0: Disabled 1: Enabled ''' try: if (en != self.reg_enabled): if en: rospy.loginfo("Enabling the 12V Regulator") GPIO.output(self.reg_en_pin, 1) self.reg_enabled = 1 else: rospy.loginfo("Disabling the 12V Regulator") GPIO.output(self.reg_en_pin, 0) self.reg_enabled = 0 except: pass def cleanup(self): ''' Cleans up the motor controller node, stopping motors and killing all threads no return value ''' rospy.loginfo("Cleaning up the motor_controller node..") self._thread_lock = False self.kill_motors() self.set_estop(1) try: GPIO.cleanup() except: pass
class Motor(object): ''' Motor class contains the methods necessary to send commands to the motor controllers for the corner and drive motors. There are many other ways of commanding the motors from the RoboClaw, we suggest trying to write your own Closed loop feedback method for the drive motors! ''' def __init__(self,config): super(Motor,self).__init__(config) self.rc = Roboclaw( config['CONTROLLER_CONFIG']['device'], config['CONTROLLER_CONFIG']['baud_rate'] ) self.rc.Open() self.address = config['MOTOR_CONFIG']['controller_address'] self.accel = [0] * 10 self.qpps = [None] * 10 self.err = [None] * 5 version = 1 for address in self.address: version = version & self.rc.ReadVersion(address)[0] print(self.rc.ReadVersion(address)[0]) if version != 0: print("[Motor__init__] Sucessfully connected to RoboClaw motor controllers") else: print("-----") raise Exception("Unable to establish connection to Roboclaw motor controllers") self.enc_min =[] self.enc_max =[] for address in self.address: self.rc.SetMainVoltages(address, int(config['BATTERY_CONFIG']['low_voltage']*10), int(config['BATTERY_CONFIG']['high_voltage']*10) ) if address == 131 or address == 132: self.rc.SetM1MaxCurrent(address, int(config['MOTOR_CONFIG']['max_corner_current']*100)) self.rc.SetM2MaxCurrent(address, int(config['MOTOR_CONFIG']['max_corner_current']*100)) self.enc_min.append(self.rc.ReadM1PositionPID(address)[-2]) self.enc_min.append(self.rc.ReadM2PositionPID(address)[-2]) self.enc_max.append(self.rc.ReadM1PositionPID(address)[-1]) self.enc_max.append(self.rc.ReadM2PositionPID(address)[-1]) else: self.rc.SetM1MaxCurrent(address, int(config['MOTOR_CONFIG']['max_drive_current']*100)) self.rc.SetM2MaxCurrent(address, int(config['MOTOR_CONFIG']['max_drive_current']*100)) self.rc.ResetEncoders(address) for address in self.address: self.rc.WriteNVM(address) for address in self.address: self.rc.ReadNVM(address) voltage = self.rc.ReadMainBatteryVoltage(0x80)[1]/10.0 if voltage >= config['BATTERY_CONFIG']['low_voltage']: print("[Motor__init__] Voltage is safe at: ",voltage, "V") else: raise Exception("Unsafe Voltage of" + voltage + " Volts") i = 0 for address in self.address: self.qpps[i] = self.rc.ReadM1VelocityPID(address)[4] self.accel[i] = int(self.qpps[i]*2) self.qpps[i+1] = self.rc.ReadM2VelocityPID(address)[4] self.accel[i+1] = int(self.qpps[i]*2) i+=2 self.errorCheck() def cornerToPosition(self,tick): ''' Method to send position commands to the corner motor :param list tick: A list of ticks for each of the corner motors to move to, if tick[i] is 0 it instead stops that motor from moving ''' speed, accel = 1000,2000 #These values could potentially need tuning still self.errorCheck() for i in range(4): index = int(math.ceil((i+1)/2.0)+2) if tick[i]: if (i % 2): self.rc.SpeedAccelDeccelPositionM2(self.address[index],accel,speed,accel,tick[i],1) else: self.rc.SpeedAccelDeccelPositionM1(self.address[index],accel,speed,accel,tick[i],1) else: if not (i % 2): self.rc.ForwardM1(self.address[index],0) else: self.rc.ForwardM2(self.address[index],0) def sendMotorDuty(self, motorID, speed): ''' Wrapper method for an easier interface to control the drive motors, sends open-loop commands to the motors :param int motorID: number that corresponds to each physical motor :param int speed: Speed for each motor, range from 0-127 ''' self.errorCheck() addr = self.address[int(motorID/2)] if speed > 0: if not motorID % 2: command = self.rc.ForwardM1 else: command = self.rc.ForwardM2 else: if not motorID % 2: command = self.rc.BackwardM1 else: command = self.rc.BackwardM2 speed = abs(int(speed * 127)) return command(addr,speed) def killMotors(self): ''' Stops all motors on Rover ''' for i in range(5): self.rc.ForwardM1(self.address[i],0) self.rc.ForwardM2(self.address[i],0) def errorCheck(self): ''' Checks error status of each motor controller, returns 0 if any errors occur ''' for i in range(5): self.err[i] = self.rc.ReadError(self.address[i])[1] for error in self.err: if error: self.killMotors() self.writeError() raise Exception("Motor controller Error", error) return 1 def writeError(self): ''' Writes the list of errors to a text file for later examination ''' f = open('errorLog.txt','a') errors = ','.join(str(e) for e in self.err) f.write('\n' + 'Errors: ' + '[' + errors + ']' + ' at: ' + str(datetime.datetime.now())) f.close()
print "Initialising roboclaw driver..." from roboclaw import Roboclaw rc = Roboclaw("/dev/roboclaw", 115200) rc.Open() rc_address = 0x80 # Get roboclaw version to test if is attached version = rc.ReadVersion(rc_address) if version[0] is False: print "Roboclaw get version failed" sys.exit() else: print repr(version[1]) # Set motor controller variables to those required by K9 rc.SetM1VelocityPID(rc_address, M1_P, M1_I, M1_D, M1_QPPS) rc.SetM2VelocityPID(rc_address, M2_P, M2_I, M2_D, M2_QPPS) rc.SetMainVoltages(rc_address, 232, 290) # 23.2V min, 29V max rc.SetPinFunctions(rc_address, 2, 0, 0) # Zero the motor encoders rc.ResetEncoders(rc_address) # Print Motor PID Settings m1pid = rc.ReadM1VelocityPID(rc_address) m2pid = rc.ReadM2VelocityPID(rc_address) print("M1 P: " + str(m1pid[1]) + ", I:" + str(m1pid[2]) + ", D:" + str(m1pid[3])) print("M2 P: " + str(m2pid[1]) + ", I:" + str(m2pid[2]) + ", D:" + str(m2pid[3])) # Print Min and Max Main Battery Settings minmaxv = rc.ReadMinMaxMainVoltages(rc_address) # get min max volts print("Min Main Battery: " + str(int(minmaxv[1]) / 10) + "V") print("Max Main Battery: " + str(int(minmaxv[2]) / 10) + "V") # Print S3, S4 and S5 Modes
from roboclaw import Roboclaw rc = Roboclaw("/dev/ttyS3", 115200) rc.Open() rc.ReadVersion(0x80) # This is the old API, use the new one below. rc.SetMinVoltageMainBattery(0x80, 110) # 11 Volts rc.ReadMinMaxMainVoltages(0x80) rc.SetMainVoltages(0x80, 110, 340) # Allowed range: 11 V - 34 V rc.SetM1MaxCurrent(0x80, 500) # 5 Amps rc.SetPWMMode(0x80, 0) # Locked Antiphase #rc.ReadPWMMode(0x80) rc.SetM1EncoderMode(0x80, 0) # No RC/Analog support + Quadrature encoder #rc.ReadEncoderModes(0x80) getConfig = rc.GetConfig(0x80) config = getConfig[1] # index zero is 1 for success, 0 for failure. config = config | 0x0003 # Packet serial mode config = config | 0x8000 # Multi-Unit mode rc.SetConfig(0x80, config) rc.SetPinFunctions(0x80, 2, 0, 0) # S3 = E-Stop, S4 = Disabled, S5 = Disabled rc.WriteNVM(0x80) rc.ReadEncM1(0x80) rc.ResetEncoders(0x80) rc.ReadEncM1(0x80) p = 15000 i = 1000