class MotorCtl(object):

   def __init__(self,motornum,motorname):
     
     self.sernum=motornum
     self.motorName=motorname
     self.mc = MotorControl() 
     try:
        self.mc.openPhidget(self.sernum)
        self.mc.waitForAttach(10000) 
     except PhidgetException as e:
        print("Phidget Exception %i: %s" % (e.code, e.details))
        print("Exiting....")
        exit(1)
 
     self.motorstate_d={'motor':0,'serial':self.sernum,'state':0,'velocity':0,'ticks':0}
   
     self.motordata=Int32MultiArray()
     self.motordata.layout.dim = [MultiArrayDimension('motor_state',4,1)] 
     self.motordata.data=self.motorstate_d.values()
     self.rsp = rospy.Publisher('phidget_motor_state',Int32MultiArray,queue_size=10)
     rospy.init_node('aux_motor_ctl',anonymous=True)
     rospy.on_shutdown(self.shutdown)
      
     robotrate=10
     r=rospy.Rate(robotrate)

   def shutdown(self):
      # Always stop the robot when shutting down the node.
      #Stop the motor
      self.mc.setVelocity(self.motorstate_d['motor'],0) 
      rospy.loginfo("Stopping the Node...")
      rospy.sleep(1)

   def procMotorCtl(self,data):

      rospy.loginfo("Got command for motor=%s command=%f",self.motorName,data.data[1])
      self.motorCmd(data.data[0],data.data[1])

   def motorCmd(self,motor,velocity):
 
       self.mc.setAcceleration(motor, 50.00)
       self.mc.setVelocity(motor, velocity) 
       
       self.motorstate_d['motor']=motor
       self.motorstate_d['velocity']=velocity
       if velocity >0:
          self.motorstate_d['state']=1
       else:
          self.motorstate_d['state']=0

       self.motorstate_d['ticks']=self.mc.getEncoderPosition(0)
       rospy.loginfo("encoder ticks=%d",self.motorstate_d['ticks'])
       print (self.motorstate_d.values())
       self.motordata.data=self.motorstate_d.values()
       self.rsp.publish(self.motordata)
 
   def runner(self):
         motorCtl = rospy.Subscriber('motor_ctl',Int32MultiArray,self.procMotorCtl)
         rospy.spin()
Exemple #2
0
    try:
        motorControl.closePhidget()
    except PhidgetException as e:
        print("Phidget Exception %i: %s" % (e.code, e.details))
        print("Exiting....")
        exit(1)
    print("Exiting....")
    exit(1)
else:
    displayDeviceInfo()

#Control the motor a bit.
print("Will now simulate motor operation....")
print("Step 1: increase acceleration to 50, set target speed at 100")
try:
    motorControl.setAcceleration(0, 50.00)
    motorControl.setVelocity(0, 100.00)
    sleep(5) #sleep for 5 seconds
except PhidgetException as e:
    print("Phidget Exception %i: %s" % (e.code, e.details))

print("Step 2: Set acceleration to 100, decrease target speed to 75")
try:
    motorControl.setAcceleration(0, 100.00)
    motorControl.setVelocity(0, 75.00)
    sleep(5) #sleep for 5 seconds
except PhidgetException as e:
    print("Phidget Exception %i: %s" % (e.code, e.details))

print("Step 3: Stop the motor by decreasing speed to 0 at 50 acceleration")
try:
    result = motors_receiver.recv_json()
    print(result)

    if 'rel' in result:
        print("Relative Settings...")
        try:
            lar = motorControlL.getAcceleration(0)
            rar = motorControlR.getAcceleration(0)

            lvr = motorControlL.getAcceleration(0)
            rvr = motorControlR.getAcceleration(0)
        except PhidgetException as e:
            print("Phidget Exception %i: %s" % (e.code, e.details))

        try:
            motorControlL.setAcceleration(0, (int(result['leftA']) + lar) )
            motorControlR.setAcceleration(0, (int(result['rightA']) + rar) )
            motorControlL.setVelocity(0, (int(result['leftV']) + lvr) )
            motorControlR.setVelocity(0, (int(result['rightV']) + rvr) )
        except PhidgetException as e:
            print("Phidget Exception %i: %s" % (e.code, e.details))

    else:
        print("Absolute Settings...")
	print("%s :: leftV = %s, rightV = %s, leftA = %s, rightA = %s" % (str(datetime.now()),result['leftV'],result['rightV'],result['leftA'],result['rightA']) )
        #fix bad accelearation values
	if result['leftA'] < Amin:
		leftA = Amax
	elif result['leftA'] > Amax:
		leftA = Amax
	else:
class IOTools:
    def __init__(self, onRobot):
        self.onRobot = onRobot

        # Camera
        self._openCam=False
        
        # Motor Control
        self._snMot=-1
        self._openMot=False
        self._attachedMot=False
        self._cur = [0, 0]

        # Servo
        self._snSer=-1
        self._openSer=False
        self._attachedSer=False
        self._limits = [0, 0]
        
        # IF Kit
        self._snIF=-1
        self._openIF=False
        self._attachedIF=False        
        self._inp = [0, 0, 0, 0, 0, 0, 0, 0]
        self._sen = [0, 0, 0, 0, 0, 0, 0, 0]
        
        # LEDs
        self._fistTime = True
        self._status = [0,0,0]
        self._mod = [8, 1, 1]
        self._rep = [-1, 5, 6]
        self._val = [True, False, False]
        self._ofs = [0, 0, 0]
        self._updaterThread = threading.Thread(target=self.__updateLED)
        self._stop = False
        self._updaterThread.setDaemon(True)
        self._updaterThread.start()

    def destroy(self):
        # LEDs
        self._stop = True
        if self._attachedIF:
            self._interfaceKit.setOutputState(0, 0)
            self._interfaceKit.setOutputState(1, 0)
            self._interfaceKit.setOutputState(2, 0)
        # Servo
        self.servoDisengage()
        # Camera
        self._openCam = False
        if self._openCam:
            self._cap.release()

    def open(self):
        self.__openIF()
        self.__openMot()
        self.__openSer()
        self.__openCam()

###################### Camera ######################
        
    def __openCam(self):
        if not os.path.exists('/dev/video0'):
            return False
        self._cap = cv2.VideoCapture()
        if not self._cap.open(-1):
            return False
        self._openCam = True

    def cameraGrab(self):
        if self._openCam:
            return self._cap.grab()
        else:
            return False

    def cameraRead(self):
        if self._openCam:
            (ret, img)=self._cap.retrieve()
            return img
        else:
            return False

    def cameraSetResolution(self, sz):
        """

        :rtype : object
        """
        if self._openCam:
            sz=sz.lower()
            if sz=='low':
                self._cap.set(3,160)
                self._cap.set(4,120)
            if sz=='medium':
                self._cap.set(3,640)
                self._cap.set(4,480)
            if sz=='high':
                self._cap.set(3,800)
                self._cap.set(4,600)
            if sz=='full':
                self._cap.set(3,1280)
                self._cap.set(4,720)

    def imshow(self, wnd, img):
        if not self.onRobot:
            if img.__class__ != numpy.ndarray:
                print "imshow - invalid image"
                return False
            else:
                cv2.imshow(wnd,img)
                cv2.waitKey(5)
        
####################### Servo ######################

    def __openSer(self):
        try:
            self._advancedServo = AdvancedServo()
        except RuntimeError as e:
            print("Servo - Runtime Exception: %s" % e.details)
            return False        

        try:
            self._advancedServo.setOnAttachHandler(self.__onAttachedSer)
            self._advancedServo.setOnDetachHandler(self.__onDetachedSer)
            self._advancedServo.setOnErrorhandler(self.__onErrorSer)
        except PhidgetException as e:
            print("Servo - Phidget Exception %i: %s" % (e.code, e.details))
            return False
        
        try:
            self._advancedServo.openPhidget()
        except PhidgetException as e:
            print("Servo - Phidget Exception %i: %s" % (e.code, e.details))
            return False
        self._openSer=True
        return True

    def __onAttachedSer(self,e):
        self._snSer = e.device.getSerialNum()
        self._advancedServo.setServoType(0, ServoTypes.PHIDGET_SERVO_HITEC_HS322HD)
        self._advancedServo.setAcceleration(0, self._advancedServo.getAccelerationMax(0))
        self._advancedServo.setVelocityLimit(0, self._advancedServo.getVelocityMax(0))
        self._limits[0] = self._advancedServo.getPositionMin(0)
        self._limits[1] = self._advancedServo.getPositionMax(0)
        print("Servo %i Attached! Range: %f - %f" % (self._snSer, self._limits[0], self._limits[1]))
        self._attachedSer=True

    def __onDetachedSer(self,e ):
        print("Servo %i Detached!" % (self._snSer))
        self._snSer = -1
        self._attachedSer=False
              
    def __onErrorSer(self, e):
        try:
            source = e.device
            print("Servo %i: Phidget Error %i: %s" % (source.getSerialNum(), e.eCode, e.description))
        except PhidgetException as e:
            print("Servo - Phidget Exception %i: %s" % (e.code, e.details))

    def __closeSer(self):
        if self._openSer==True:
            self.servoDisengage()
            self._advancedServo.closePhidget()

    def servoEngage(self):
        if self._attachedSer==True:
            self._advancedServo.setEngaged(0, True)

    def servoDisengage(self):
        if self._attachedSer==True:
            self._advancedServo.setEngaged(0, False)

    def servoSet(self, pos):
        if self._attachedSer==True:
            self._advancedServo.setPosition(0, min(max(pos,self._limits[0]),self._limits[1]))
        
############### Motor Control ######################

    def __openMot(self):
        try:
            self._motorControl = MotorControl()
        except RuntimeError as e:
            print("Motor Control - Runtime Exception: %s" % e.details)
            return False        

        try:
            self._motorControl.setOnAttachHandler(self.__onAttachedMot)
            self._motorControl.setOnDetachHandler(self.__onDetachedMot)
            self._motorControl.setOnErrorhandler(self.__onErrorMot)
            self._motorControl.setOnCurrentChangeHandler(self.__onCurrentChangedMot)
        except PhidgetException as e:
            print("Motor Control - Phidget Exception %i: %s" % (e.code, e.details))
            return False
        
        try:
            self._motorControl.openPhidget()
        except PhidgetException as e:
            print("Motor Control - Phidget Exception %i: %s" % (e.code, e.details))
            return False
        self._openMot=True
        return True

    def __onAttachedMot(self,e):
        self._snMot = e.device.getSerialNum()
        print("Motor Control %i Attached!" % (self._snMot))
        self._attachedMot=True

    def __onDetachedMot(self,e ):
        print("Motor Control %i Detached!" % (self._snMot))
        self._snMot = -1
        self._attachedMot=False
              
    def __onErrorMot(self, e):
        try:
            source = e.device
            print("Motor Control %i: Phidget Error %i: %s" % (source.getSerialNum(), e.eCode, e.description))
        except PhidgetException as e:
            print("Motor Control - Phidget Exception %i: %s" % (e.code, e.details))

    def __onCurrentChangedMot(self, e):
        self._cur[e.index] = e.current

    def __closeMot(self):
        if self._openMot==True:
            self._motorControl.closePhidget()

    def setMotors(self, speed1=0.0, speed2=0.0, acceleration1=100.0, acceleration2=100.0 ):
        if self._openMot==True:
            self._motorControl.setAcceleration(0, acceleration1)
            self._motorControl.setVelocity(0, speed1)
            self._motorControl.setAcceleration(1, acceleration2)
            self._motorControl.setVelocity(1, speed2)
            
############### Interface Kit ######################

    def __closeIF(self):
        if self._openIF==True:
            self._interfaceKit.closePhidget()
            
    def __openIF(self):
        try:
            self._interfaceKit = InterfaceKit()
        except RuntimeError as e:
            print("IF Kit - Runtime Exception: %s" % e.details)
            return False
        try:
            self._interfaceKit.setOnAttachHandler(self.__onAttachedIF)
            self._interfaceKit.setOnDetachHandler(self.__onDetachedIF)
            self._interfaceKit.setOnErrorhandler(self.__onErrorIF)
            self._interfaceKit.setOnInputChangeHandler(self.__onInputChangedIF)
            self._interfaceKit.setOnSensorChangeHandler(self.__onSensorChangedIF)
        except PhidgetException as e:
            print("IF Kit - Phidget Exception %i: %s" % (e.code, e.details))
            return False
        try:
            self._interfaceKit.openPhidget()
        except PhidgetException as e:
            print("IF Kit - Phidget Exception %i: %s" % (e.code, e.details))
            return False
        self._openIF=True
        return True
        
    def __onAttachedIF(self,e):
        self._snIF = e.device.getSerialNum()
        print("InterfaceKit %i Attached!" % (self._snIF))
        self._attachedIF=True
        if self._fistTime:
            for i in range(0,3):
                self._interfaceKit.setOutputState(0, 1)
                self._interfaceKit.setOutputState(1, 1)
                self._interfaceKit.setOutputState(2, 1)
                time.sleep(0.1)
                self._interfaceKit.setOutputState(0, 0)
                self._interfaceKit.setOutputState(1, 0)
                self._interfaceKit.setOutputState(2, 0)
                time.sleep(0.1)
        self._fistTime = False

    def __onDetachedIF(self,e ):
        print("InterfaceKit %i Detached!" % (self._snIF))
        self._snIF = -1
        self._inp = [0, 0, 0, 0, 0, 0, 0, 0]
        self._sen = [0, 0, 0, 0, 0, 0, 0, 0]
        self._attachedIF=False
              
    def __onErrorIF(self, e):
        try:
            source = e.device
            print("InterfaceKit %i: Phidget Error %i: %s" % (source.getSerialNum(), e.eCode, e.description))
        except PhidgetException as e:
            print("IF Kit - Phidget Exception %i: %s" % (e.code, e.details))

    def __onSensorChangedIF(self, e):
        self._sen[e.index] = e.value

    def __onInputChangedIF(self, e):
        self._inp[e.index] = e.state

    def getSensors(self):
        """

        :rtype : object
        """
        return self._sen;

    def getInputs(self):
        return self._inp;

################ LEDs #######################

    def __updateLED(self):
        t=0
        while self._stop==False:
            t=(t+1)%100
            for i in range(0,3):
                self._status[i]=((t+self._ofs[i])%self._mod[i]==0) and self._val[i] and bool(self._rep[i])
                self._rep[i]=self._rep[i]-int(self._rep[i]>0 and self._status[i])
                if self._attachedIF:
                    self._interfaceKit.setOutputState(i, self._status[i])
            time.sleep(0.15)
            
    def __setModeLED(self, i, mode, hz=2, cnt=1, ofs=0):
        if mode=='on':
            self._rep[i]=-1
            self._val[i]=True
            self._ofs[i]=0
            self._mod[i]=1
        if mode=='off':
            self._rep[i]=-1
            self._val[i]=False
            self._ofs[i]=0
            self._mod[i]=1
        if mode=='flash':
            hz=min(max(hz,1),100)
            self._rep[i]=min(max(cnt,1),20)
            self._val[i]=True
            self._ofs[i]=min(max(ofs,0),hz)
            self._mod[i]=hz

    def setStatus(self, mode, hz=2, cnt=1, ofs=0):
        self.__setModeLED(1,mode, hz, cnt, ofs)            
                
    def setError(self, mode, hz=2, cnt=1, ofs=0):
        self.__setModeLED(2,mode, hz, cnt, ofs)

    def setSemaphor(self):
        self.__setModeLED(1,'flash', 2, 6, 0)
        self.__setModeLED(2,'flash', 2, 6, 1)
Exemple #5
0
    print "waitForAttach() failed"
    print "code: %d" % e.code
    print "message", e.message
    
    raise

if motorControl.isAttached():
    print "motor control attached"
else:
    print "motor attach Failed"

encoder.setPosition(whichEncoder, 0)
encoder.setEnabled(whichEncoder, True)
averageAcceleration = (motorControl.getAccelerationMax(whichMotor) - motorControl.getAccelerationMin(whichMotor)) * 0.9
print "averageAcceleration", averageAcceleration
motorControl.setAcceleration(whichMotor, averageAcceleration)

position = encoder.getPosition(whichEncoder)
print "Starting position", position
startTime = time.clock()
motorControl.setVelocity(whichMotor, velocity)
try:
    while position < pulsesWanted:
        position = encoder.getPosition(whichEncoder) * encoderSign
        print "Position", position, "Current", motorControl.getCurrent(whichMotor), "\r",
    
except:
    print "Aborted"
    pass
    
endTime = time.clock()
Exemple #6
0
    try:
        motorControl.closePhidget()
    except PhidgetException as e:
        print("Phidget Exception %i: %s" % (e.code, e.details))
        print("Exiting....")
        exit(1)
    print("Exiting....")
    exit(1)
else:
    displayDeviceInfo()

#Control the motor a bit.
print("Will now simulate motor operation....")
print("Step 1: increase acceleration to 50, set target speed at 100")
try:
    motorControl.setAcceleration(0, 50.00)
    motorControl.setVelocity(0, 100.00)
    sleep(5)  #sleep for 5 seconds
except PhidgetException as e:
    print("Phidget Exception %i: %s" % (e.code, e.details))

print("Step 2: Set acceleration to 100, decrease target speed to 75")
try:
    motorControl.setAcceleration(0, 100.00)
    motorControl.setVelocity(0, 75.00)
    sleep(5)  #sleep for 5 seconds
except PhidgetException as e:
    print("Phidget Exception %i: %s" % (e.code, e.details))

print("Step 3: Stop the motor by decreasing speed to 0 at 50 acceleration")
try:
Exemple #7
0
class IOTools:
    def __init__(self, onRobot):
        self.onRobot = onRobot

        # Camera
        self._openCam = False

        # Motor Control
        self._snMot = -1
        self._openMot = False
        self._attachedMot = False
        self._cur = [0, 0]

        # Servo
        self._snSer = -1
        self._openSer = False
        self._attachedSer = False
        self._limits = [0, 0]

        # IF Kit
        self._snIF = -1
        self._openIF = False
        self._attachedIF = False
        self._inp = [0, 0, 0, 0, 0, 0, 0, 0]
        self._sen = [0, 0, 0, 0, 0, 0, 0, 0]

        # LEDs
        self._fistTime = True
        self._status = [0, 0, 0]
        self._mod = [8, 1, 1]
        self._rep = [-1, 5, 6]
        self._val = [True, False, False]
        self._ofs = [0, 0, 0]
        self._updaterThread = threading.Thread(target=self.__updateLED)
        self._stop = False
        self._updaterThread.setDaemon(True)
        self._updaterThread.start()

    def destroy(self):
        # LEDs
        self._stop = True
        if self._attachedIF:
            self._interfaceKit.setOutputState(0, 0)
            self._interfaceKit.setOutputState(1, 0)
            self._interfaceKit.setOutputState(2, 0)
        # Servo
        self.servoDisengage()
        # Camera
        self._openCam = False
        if self._openCam:
            self._cap.release()

    def open(self):
        self.__openIF()
        self.__openMot()
        self.__openSer()
        self.__openCam()

###################### Camera ######################

    def __openCam(self):
        if not os.path.exists('/dev/video0'):
            return False
        self._cap = cv2.VideoCapture()
        if not self._cap.open(-1):
            return False
        self._openCam = True

    def cameraGrab(self):
        if self._openCam:
            return self._cap.grab()
        else:
            return False

    def cameraRead(self):
        if self._openCam:
            (ret, img) = self._cap.retrieve()
            return img
        else:
            return False

    def cameraSetResolution(self, sz):
        if self._openCam:
            sz = sz.lower()
            if sz == 'low':
                self._cap.set(3, 160)
                self._cap.set(4, 120)
            if sz == 'medium':
                self._cap.set(3, 640)
                self._cap.set(4, 480)
            if sz == 'high':
                self._cap.set(3, 800)
                self._cap.set(4, 600)
            if sz == 'full':
                self._cap.set(3, 1280)
                self._cap.set(4, 720)

    def imshow(self, wnd, img):
        if not self.onRobot:
            if img.__class__ != numpy.ndarray:
                print "imshow - invalid image"
                return False
            else:
                cv2.imshow(wnd, img)
                cv2.waitKey(5)

####################### Servo ######################

    def __openSer(self):
        try:
            self._advancedServo = AdvancedServo()
        except RuntimeError as e:
            print("Servo - Runtime Exception: %s" % e.details)
            return False

        try:
            self._advancedServo.setOnAttachHandler(self.__onAttachedSer)
            self._advancedServo.setOnDetachHandler(self.__onDetachedSer)
            self._advancedServo.setOnErrorhandler(self.__onErrorSer)
        except PhidgetException as e:
            print("Servo - Phidget Exception %i: %s" % (e.code, e.details))
            return False

        try:
            self._advancedServo.openPhidget()
        except PhidgetException as e:
            print("Servo - Phidget Exception %i: %s" % (e.code, e.details))
            return False
        self._openSer = True
        return True

    def __onAttachedSer(self, e):
        self._snSer = e.device.getSerialNum()
        self._advancedServo.setServoType(
            0, ServoTypes.PHIDGET_SERVO_HITEC_HS322HD)
        self._advancedServo.setAcceleration(
            0, self._advancedServo.getAccelerationMax(0))
        self._advancedServo.setVelocityLimit(
            0, self._advancedServo.getVelocityMax(0))
        self._limits[0] = self._advancedServo.getPositionMin(0)
        self._limits[1] = self._advancedServo.getPositionMax(0)
        print("Servo %i Attached! Range: %f - %f" %
              (self._snSer, self._limits[0], self._limits[1]))
        self._attachedSer = True

    def __onDetachedSer(self, e):
        print("Servo %i Detached!" % (self._snSer))
        self._snSer = -1
        self._attachedSer = False

    def __onErrorSer(self, e):
        try:
            source = e.device
            print("Servo %i: Phidget Error %i: %s" %
                  (source.getSerialNum(), e.eCode, e.description))
        except PhidgetException as e:
            print("Servo - Phidget Exception %i: %s" % (e.code, e.details))

    def __closeSer(self):
        if self._openSer == True:
            self.servoDisengage()
            self._advancedServo.closePhidget()

    def servoEngage(self):
        if self._attachedSer == True:
            self._advancedServo.setEngaged(0, True)

    def servoDisengage(self):
        if self._attachedSer == True:
            self._advancedServo.setEngaged(0, False)

    def servoSet(self, pos):
        if self._attachedSer == True:
            self._advancedServo.setPosition(
                0, min(max(pos, self._limits[0]), self._limits[1]))

############### Motor Control ######################

    def __openMot(self):
        try:
            self._motorControl = MotorControl()
        except RuntimeError as e:
            print("Motor Control - Runtime Exception: %s" % e.details)
            return False

        try:
            self._motorControl.setOnAttachHandler(self.__onAttachedMot)
            self._motorControl.setOnDetachHandler(self.__onDetachedMot)
            self._motorControl.setOnErrorhandler(self.__onErrorMot)
            self._motorControl.setOnCurrentChangeHandler(
                self.__onCurrentChangedMot)
        except PhidgetException as e:
            print("Motor Control - Phidget Exception %i: %s" %
                  (e.code, e.details))
            return False

        try:
            self._motorControl.openPhidget()
        except PhidgetException as e:
            print("Motor Control - Phidget Exception %i: %s" %
                  (e.code, e.details))
            return False
        self._openMot = True
        return True

    def __onAttachedMot(self, e):
        self._snMot = e.device.getSerialNum()
        print("Motor Control %i Attached!" % (self._snMot))
        self._attachedMot = True

    def __onDetachedMot(self, e):
        print("Motor Control %i Detached!" % (self._snMot))
        self._snMot = -1
        self._attachedMot = False

    def __onErrorMot(self, e):
        try:
            source = e.device
            print("Motor Control %i: Phidget Error %i: %s" %
                  (source.getSerialNum(), e.eCode, e.description))
        except PhidgetException as e:
            print("Motor Control - Phidget Exception %i: %s" %
                  (e.code, e.details))

    def __onCurrentChangedMot(self, e):
        self._cur[e.index] = e.current

    def __closeMot(self):
        if self._openMot == True:
            self._motorControl.closePhidget()

    def setMotors(self,
                  speed1=0.0,
                  speed2=0.0,
                  acceleration1=100.0,
                  acceleration2=100.0):
        if self._openMot == True:
            self._motorControl.setAcceleration(0, acceleration1)
            self._motorControl.setVelocity(0, speed1)
            self._motorControl.setAcceleration(1, acceleration2)
            self._motorControl.setVelocity(1, speed2)

############### Interface Kit ######################

    def __closeIF(self):
        if self._openIF == True:
            self._interfaceKit.closePhidget()

    def __openIF(self):
        try:
            self._interfaceKit = InterfaceKit()
        except RuntimeError as e:
            print("IF Kit - Runtime Exception: %s" % e.details)
            return False
        try:
            self._interfaceKit.setOnAttachHandler(self.__onAttachedIF)
            self._interfaceKit.setOnDetachHandler(self.__onDetachedIF)
            self._interfaceKit.setOnErrorhandler(self.__onErrorIF)
            self._interfaceKit.setOnInputChangeHandler(self.__onInputChangedIF)
            self._interfaceKit.setOnSensorChangeHandler(
                self.__onSensorChangedIF)
        except PhidgetException as e:
            print("IF Kit - Phidget Exception %i: %s" % (e.code, e.details))
            return False
        try:
            self._interfaceKit.openPhidget()
        except PhidgetException as e:
            print("IF Kit - Phidget Exception %i: %s" % (e.code, e.details))
            return False
        self._openIF = True
        return True

    def __onAttachedIF(self, e):
        self._snIF = e.device.getSerialNum()
        print("InterfaceKit %i Attached!" % (self._snIF))
        self._attachedIF = True
        if self._fistTime:
            for i in range(0, 3):
                self._interfaceKit.setOutputState(0, 1)
                self._interfaceKit.setOutputState(1, 1)
                self._interfaceKit.setOutputState(2, 1)
                time.sleep(0.1)
                self._interfaceKit.setOutputState(0, 0)
                self._interfaceKit.setOutputState(1, 0)
                self._interfaceKit.setOutputState(2, 0)
                time.sleep(0.1)
        self._fistTime = False

    def __onDetachedIF(self, e):
        print("InterfaceKit %i Detached!" % (self._snIF))
        self._snIF = -1
        self._inp = [0, 0, 0, 0, 0, 0, 0, 0]
        self._sen = [0, 0, 0, 0, 0, 0, 0, 0]
        self._attachedIF = False

    def __onErrorIF(self, e):
        try:
            source = e.device
            print("InterfaceKit %i: Phidget Error %i: %s" %
                  (source.getSerialNum(), e.eCode, e.description))
        except PhidgetException as e:
            print("IF Kit - Phidget Exception %i: %s" % (e.code, e.details))

    def __onSensorChangedIF(self, e):
        self._sen[e.index] = e.value

    def __onInputChangedIF(self, e):
        self._inp[e.index] = e.state

    def getSensors(self):
        return self._sen

    def getInputs(self):
        return self._inp

################ LEDs #######################

    def __updateLED(self):
        t = 0
        while self._stop == False:
            t = (t + 1) % 100
            for i in range(0, 3):
                self._status[i] = ((t + self._ofs[i]) % self._mod[i]
                                   == 0) and self._val[i] and bool(
                                       self._rep[i])
                self._rep[i] = self._rep[i] - int(self._rep[i] > 0
                                                  and self._status[i])
                if self._attachedIF:
                    self._interfaceKit.setOutputState(i, self._status[i])
            time.sleep(0.15)

    def __setModeLED(self, i, mode, hz=2, cnt=1, ofs=0):
        if mode == 'on':
            self._rep[i] = -1
            self._val[i] = True
            self._ofs[i] = 0
            self._mod[i] = 1
        if mode == 'off':
            self._rep[i] = -1
            self._val[i] = False
            self._ofs[i] = 0
            self._mod[i] = 1
        if mode == 'flash':
            hz = min(max(hz, 1), 100)
            self._rep[i] = min(max(cnt, 1), 20)
            self._val[i] = True
            self._ofs[i] = min(max(ofs, 0), hz)
            self._mod[i] = hz

    def setStatus(self, mode, hz=2, cnt=1, ofs=0):
        self.__setModeLED(1, mode, hz, cnt, ofs)

    def setError(self, mode, hz=2, cnt=1, ofs=0):
        self.__setModeLED(2, mode, hz, cnt, ofs)

    def setSemaphor(self):
        self.__setModeLED(1, 'flash', 2, 6, 0)
        self.__setModeLED(2, 'flash', 2, 6, 1)
Exemple #8
0
class MotorCtl(object):
    def __init__(self, motornum, motorname):

        self.sernum = motornum
        self.motorName = motorname
        self.mc = MotorControl()
        try:
            self.mc.openPhidget(self.sernum)
            self.mc.waitForAttach(10000)
        except PhidgetException as e:
            print("Phidget Exception %i: %s" % (e.code, e.details))
            print("Exiting....")
            exit(1)

        self.motorstate_d = {
            'motor': 0,
            'serial': self.sernum,
            'state': 0,
            'velocity': 0,
            'ticks': 0
        }

        self.motordata = Int32MultiArray()
        self.motordata.layout.dim = [MultiArrayDimension('motor_state', 4, 1)]
        self.motordata.data = self.motorstate_d.values()
        self.rsp = rospy.Publisher('phidget_motor_state',
                                   Int32MultiArray,
                                   queue_size=10)
        rospy.init_node('aux_motor_ctl', anonymous=True)
        rospy.on_shutdown(self.shutdown)

        robotrate = 10
        r = rospy.Rate(robotrate)

    def shutdown(self):
        # Always stop the robot when shutting down the node.
        #Stop the motor
        self.mc.setVelocity(self.motorstate_d['motor'], 0)
        rospy.loginfo("Stopping the Node...")
        rospy.sleep(1)

    def procMotorCtl(self, data):

        rospy.loginfo("Got command for motor=%s command=%f", self.motorName,
                      data.data[1])
        self.motorCmd(data.data[0], data.data[1])

    def motorCmd(self, motor, velocity):

        self.mc.setAcceleration(motor, 50.00)
        self.mc.setVelocity(motor, velocity)

        self.motorstate_d['motor'] = motor
        self.motorstate_d['velocity'] = velocity
        if velocity > 0:
            self.motorstate_d['state'] = 1
        else:
            self.motorstate_d['state'] = 0

        self.motorstate_d['ticks'] = self.mc.getEncoderPosition(0)
        rospy.loginfo("encoder ticks=%d", self.motorstate_d['ticks'])
        print(self.motorstate_d.values())
        self.motordata.data = self.motorstate_d.values()
        self.rsp.publish(self.motordata)

    def runner(self):
        motorCtl = rospy.Subscriber('motor_ctl', Int32MultiArray,
                                    self.procMotorCtl)
        rospy.spin()
motor_current_output = []
torque_sensor_output = []
position_output = []
velocity_output = []

# Main loop
for direction in inputs.keys():
	if direction == 'Open':
		difference = position_difference[0]
	else:
		difference = position_difference[1]

	while difference > 1:
		# Set accleeration to 50%
		try:
			motor.setAcceleration(0, 50)
		except PhidgetException as error:
			print ('Phidget Error %i: %s' % (error.code, error.details))

		# Speed & error depend on the direction we're going
		if direction == 'Open':
			try:
				motor.setVelocity(0, PWM_duty_cycle[0]) # Set the OPEN speed as positive
			except PhidgetException as error:
				print ('Phidget Error %i: %s' % (error.code, error.details))
			commanded_position = commanded_position[0] # Set using the OPEN index

		else:
			try:
				motor.setVelocity(0, -PWM_duty_cycle[1]) # Set the CLOSE speed as negative
			except PhidgetException as error:
Exemple #10
0
    result = motors_receiver.recv_json()
    print(result)

    if 'rel' in result:
        print("Relative Settings...")
        try:
            lar = motorControlL.getAcceleration(0)
            rar = motorControlR.getAcceleration(0)

            lvr = motorControlL.getAcceleration(0)
            rvr = motorControlR.getAcceleration(0)
        except PhidgetException as e:
            print("Phidget Exception %i: %s" % (e.code, e.details))

        try:
            motorControlL.setAcceleration(0, (int(result['leftA']) + lar))
            motorControlR.setAcceleration(0, (int(result['rightA']) + rar))
            motorControlL.setVelocity(0, (int(result['leftV']) + lvr))
            motorControlR.setVelocity(0, (int(result['rightV']) + rvr))
        except PhidgetException as e:
            print("Phidget Exception %i: %s" % (e.code, e.details))

    else:
        print("Absolute Settings...")
        print("%s :: leftV = %s, rightV = %s, leftA = %s, rightA = %s" %
              (str(datetime.now()), result['leftV'], result['rightV'],
               result['leftA'], result['rightA']))
        #fix bad accelearation values
        if result['leftA'] < Amin:
            leftA = Amax
        elif result['leftA'] > Amax: