def displayspeed(): enc1 = roboclaw.ReadEncM1(address) enc2 = roboclaw.ReadEncM2(address) speed1 = roboclaw.ReadSpeedM1(address) speed2 = roboclaw.ReadSpeedM2(address) print("Encoder1:"), if (enc1[0] == 1): print enc1[1], print format(enc1[2], '02x'), else: print "failed", print "Encoder2:", if (enc2[0] == 1): print enc2[1], print format(enc2[2], '02x'), else: print "failed ", print "Speed1:", if (speed1[0]): print speed1[1], else: print "failed", print("Speed2:"), if (speed2[0]): print speed2[1] else: print "failed "
def readAngles(): kneeAngle = int(rc.ReadEncM1(config.address)[1]) hipAngle = int(rc.ReadEncM2(config.address)[1]) heelAngle = readPot() return kneeAngle, hipAngle, heelAngle
def update(self): """ Sends """ signal.setitimer( signal.ITIMER_REAL, 0.25 ) # set the watchdog for 0.25 seconds (SIGALARM_handler() will be # called if this function takes too long) try: # Publish encoder readings if self.m1_enc_pub is not None: response_1 = roboclaw.ReadEncM1(self.address) response_2 = roboclaw.ReadEncM2(self.address) if response_1[0] == 0 or response_1[0] == 0: rospy.logerr( str((self.name, os.getpid())) + ": error returned from encoder reading: " + str(response_1) + " " + str(response_2)) else: m1_msg = Int32() m1_msg.data = int(response_1[1]) m2_msg = Int32() m2_msg.data = int(response_2[1]) self.m1_enc_pub.publish(m1_msg) self.m2_enc_pub.publish(m2_msg) # Publish current readings if self.m1_enc_pub is not None: currents = roboclaw.ReadCurrents(self.address) if currents[0] == 0: rospy.logerr( str((self.name, os.getpid())) + ": error returned from current reading: " + str(currents)) else: m1_amp_msg = Float32() m1_amp_msg.data = currents[1] / 100.0 m2_amp_msg = Float32() m2_amp_msg.data = currents[2] / 100.0 self.m1_amp_pub.publish(m1_amp_msg) self.m2_amp_pub.publish(m2_amp_msg) bat_voltage = roboclaw.ReadMainBatteryVoltage(self.address) if bat_voltage[0] == 0: rospy.logerr( str((self.name, os.getpid())) + ": error returned from battery voltage reading: " + str(bat_voltage)) else: bat_volt_msg = Float32() bat_volt_msg.data = bat_voltage[1] / 10.0 self.battery_voltage_pub.publish(bat_volt_msg) # Timeout if no command recieved for more than TIMEOUT_TIME if int(round( time.time() * 1000)) - self.timeout > self.TIMEOUT_TIME: roboclaw.ForwardBackwardM1(self.address, 64) roboclaw.ForwardBackwardM2(self.address, 64) else: # PWM control if self.m1_pwm is not None: roboclaw.ForwardBackwardM1(self.address, self.m1_pwm) # roboclaw.DutyM1(self.address, (self.m1_pwm - 64) * int(32767 / 64)) self.m1_pwm = None if self.m2_pwm is not None: roboclaw.ForwardBackwardM2(self.address, self.m2_pwm) # roboclaw.DutyM2(self.address, (self.m2_pwm - 64) * int(32768 / 64)) self.m2_pwm = None # Velocity control if self.m1_vel is not None: # if the commanded velocity is 0 then turn the motors off instead of sending a zero speed. # This is to keep the motors from overheating, we once left the rover still on a slight slope for # a few minutes and the motors overheated trying to keep the rover still. if self.m1_vel == 0: roboclaw.DutyM1(self.address, 0) else: roboclaw.SpeedM1(self.address, self.m1_vel) self.m1_vel = None if self.m2_vel is not None: if self.m2_vel == 0: roboclaw.DutyM2(self.address, 0) else: roboclaw.SpeedM2(self.address, self.m2_vel) self.m2_vel = None # Position control if self.m1_pos is not None: roboclaw.SpeedAccelDeccelPositionM1( self.address, 0, self.QPPS, 0, self.m1_pos, False) self.m1_pos = None if self.m2_pos is not None: roboclaw.SpeedAccelDeccelPositionM2( self.address, 0, self.QPPS, 0, self.m2_pos, False) self.m2_pos = None # this exception will be raised by the SIGALRM handler if sending/recieving data from the roboclaw took too long except Exception as e: rospy.logerr("SIGLARAM: " + str((self.name, os.getpid())) + ": " + str(e))
def update(self): for i, pub in enumerate(self.enc_publishers): if pub is None: continue address = self.parameters[i]['address'] if self.parameters[i]['motor_num'] == 1: response = roboclaw.ReadEncM1(address) elif self.parameters[i]['motor_num'] == 2: response = roboclaw.ReadEncM2(address) else: continue msg = Int32() msg.data = int(response[1]) pub.publish(msg) for i, t in enumerate(self.timeouts): if self.enc_publishers[i] is None: continue if int(round(time.time() * 1000)) - t > self.TIMEOUT_TIME: address = self.parameters[i]['address'] if self.parameters[i]['motor_num'] == 1: roboclaw.ForwardBackwardM1(address, 64) elif self.parameters[i]['motor_num'] == 2: roboclaw.ForwardBackwardM2(address, 64) else: continue ''' for i, pwm in enumerate(self.pwms): if pwm is None: continue if self.parameters[i] is not None: address = self.parameters[i]['address'] if self.parameters[i]['motor_num'] == 1: roboclaw.ForwardBackwardM1(address, pwm) elif self.parameters[i]['motor_num'] == 2: roboclaw.ForwardBackwardM2(address, pwm) self.pwms[i] = None ''' for i, vel in enumerate(self.velocities): if vel is None: continue #print('found_vel: ' + str(vel)) if self.parameters[i] is not None: address = self.parameters[i]['address'] #print('address: ' + str(address)) if self.parameters[i]['motor_num'] == 1: #print('setting m1 speed') if roboclaw.SpeedM1(address, vel): #print('m1_set') pass else: #print('m1_failed') pass elif self.parameters[i]['motor_num'] == 2: #print('setting m2 speed') if roboclaw.SpeedM2(address, vel): #print('m1_set') pass else: #print('m1_failed') pass self.velocities[i] = None '''
def zero_func(): """ Finds values for each encoder to be used as position reference. Arguments: (None) Returns: ref -- list of reference encoder values """ ref = [0]*3 ref[0] = rc.ReadEncM1(address)[1] ref[1] = rc.ReadEncM2(address)[1] ref[2] = 0 # read Pot Value return ref # make this a global? Need to deal with # user input --> then start trying to reach defined positions # while running, return angle values with feedback, tighten string and get setup finalized # when ready, return message that everything is good # options: go to neutral state, or directly into primed state def push_into_position_func(): """ Applies constant torques to push apparatus into a specified position. Reads config file to get target positions.