00001 from threading import Thread 00002 from BAL.Devices.RICOpenLoopMotor import OpenLoopMotor 00003 from BAL.Devices.RiCBattery import RiCBattery 00004 from BAL.Devices.RiCDiffCloseLoop import RiCDiffCloseLoop 00005 from BAL.Devices.RiCGPS import RiCGPS 00006 from BAL.Devices.RiCIMU import RiCIMU 00007 from BAL.Devices.RiCOpenDiff import RiCOpenDiff 00008 from BAL.Devices.RiCPPM import RiCPPM 00009 from BAL.Devices.RiCRelay import RiCRelay 00010 from BAL.Devices.RiCSwitch import RiCSwitch 00011 from BAL.Devices.RiCURF import RiCURF 00012 from BAL.Header.Response.BatteryParamResponse import BatteryParamResponse 00013 from BAL.Header.Response.CloseLoopMotorTwoEncBuildResponse import CloseLoopMotorTwoEncBuildResponse 00014 from BAL.Header.Response.IMUParamResponse import IMUParamResponse 00015 from BAL.Header.Response.ParamBuildResponse import EngineCL, EngineCL2 00016 from BAL.Header.Response.closeDiffFourParamResponse import CloseDIffFourParamResponse 00017 from BAL.Header.Response.closeDiffParamResponse import CloseDiffParamResponse 00018 from BAL.Header.Response.emergencySwitchParamResponse import EmergencySwitchParamResponse 00019 from BAL.Header.Response.gpsParamResponse import GPSParamResponse 00020 from BAL.Header.Response.openLoopMotorParamResponse import OpenLoopMotorParamResponse 00021 from BAL.Header.Response.ppmParamResponse import PPMParamResponse 00022 from BAL.Header.Response.relayParamResponse import RelayParamResponse 00023 from BAL.Header.Response.switchParamResponse import SwitchParamResponse 00024 from BAL.Header.Response.urfParamResponse import URFParamResponse 00025 00026 __author__ = 'tom1231' 00027 00028 import rospy 00029 from BAL.Header.Requests.finishBuildingRequest import FinishBuildingRequest, ID_REQ 00030 from BAL.Devices.RiCCloseLoopMotor import RiCCloseLoopMotor 00031 from BAL.Devices.RiCServo import RiCServo 00032 from BAL.Header.Response.BuildServoResponse import BuildServoResponse 00033 from BAL.Header.Response.closeLoopMotorBuildResponse import CloseLoopMotorBuildResponse 00034 from BAL.Header.Response.ackResponse import ACKResponse 00035 from BAL.Handlers.incomingHandler import ACK_RES 00036 from BAL.Handlers.serialWriteHandler import HEADER_START 00037 00038 00039 class DeviceBuilder: 00040 def __init__(self, param, output, input, incomingDataHandle): 00041 self._param = param 00042 self._output = output 00043 self._input = input 00044 self._incomingDataHandler = incomingDataHandle 00045 self._allDevs = dict() 00046 self._allDevs['servos'] = [] 00047 self._allDevs['motorsCl'] = [] 00048 self._allDevs['motorsOl'] = [] 00049 self._allDevs['urf'] = [] 00050 self._allDevs['switch'] = [] 00051 self._allDevs['diff'] = [] 00052 self._allDevs['imu'] = [] 00053 self._allDevs['relay'] = [] 00054 self._allDevs['gps'] = [] 00055 self._allDevs['ppm'] = [] 00056 self._allDevs['battery'] = [] 00057 00058 def createServos(self): 00059 servoAmount = self._param.getServoNum() 00060 for servoNum in xrange(servoAmount): 00061 rospy.loginfo("Configuring servo: %s", self._param.getServoName(servoNum)) 00062 self._allDevs['servos'].append(RiCServo(self._param, servoNum, self._output)) 00063 servoTORic = BuildServoResponse(servoNum, self._param) 00064 self._output.writeAndWaitForAck(servoTORic.dataTosend(), servoNum) 00065 rospy.loginfo("Servo: %s is ready ", self._param.getServoName(servoNum)) 00066 00067 def createCLMotors(self): 00068 closeMotorsAmount = self._param.getCloseLoopMotorSize() 00069 for motorNum in xrange(closeMotorsAmount): 00070 rospy.loginfo("Configuring motor: %s", self._param.getCloseLoopMotorName(motorNum)) 00071 motor = RiCCloseLoopMotor(motorNum, self._param, self._output) 00072 self._allDevs['motorsCl'].append(motor) 00073 if self._param.getCloseLoopMotorEncoderType(motorNum) == 1: 00074 toSend = CloseLoopMotorBuildResponse(motorNum, self._param, EngineCL) 00075 else: 00076 toSend = CloseLoopMotorTwoEncBuildResponse(motorNum, self._param, EngineCL2) 00077 self._output.writeAndWaitForAck(toSend.dataTosend(), motorNum) 00078 rospy.loginfo("Motor: %s is ready", self._param.getCloseLoopMotorName(motorNum)) 00079 00080 def createDiff(self): 00081 if self._param.isInitCloseDiff(): 00082 rospy.loginfo("Configuring differential drive: %s", self._param.getCloseDiffName()) 00083 diff = RiCDiffCloseLoop(self._param, self._output) 00084 self._allDevs['diff'].append(diff) 00085 toSend = CloseDiffParamResponse(0, self._param) 00086 self._output.writeAndWaitForAck(toSend.dataTosend(), 0) 00087 rospy.loginfo("Differential drive: %s is ready", self._param.getCloseDiffName()) 00088 00089 def createDiffFour(self): 00090 if self._param.isCloseDiffFourInit(): 00091 rospy.loginfo("Configuring differential drive: %s", self._param.getCloseDiffName()) 00092 diff = RiCDiffCloseLoop(self._param, self._output) 00093 self._allDevs['diff'].append(diff) 00094 toSend = CloseDIffFourParamResponse(0, self._param) 00095 self._output.writeAndWaitForAck(toSend.dataTosend(), 0) 00096 rospy.loginfo("Differential drive: %s is ready", self._param.getCloseDiffName()) 00097 00098 def createURF(self): 00099 URFAmount = self._param.getURFNum() 00100 for urfId in xrange(URFAmount): 00101 rospy.loginfo("Configuring URF: %s", self._param.getURFName(urfId)) 00102 urf = RiCURF(urfId, self._param, self._output) 00103 self._allDevs['urf'].append(urf) 00104 self._output.writeAndWaitForAck(URFParamResponse(urf.getType(), urfId, self._param).dataTosend(), urfId) 00105 rospy.loginfo("URF: %s is ready", self._param.getURFName(urfId)) 00106 00107 def createSwitchs(self): 00108 switchAmount = self._param.getSwitchSize() 00109 for switchNum in xrange(switchAmount): 00110 rospy.loginfo("Configuring switch: %s", self._param.getSwitchName(switchNum)) 00111 switch = RiCSwitch(switchNum, self._param, self._output) 00112 self._allDevs['switch'].append(switch) 00113 self._output.writeAndWaitForAck(SwitchParamResponse(switchNum, self._param).dataTosend(), switchNum) 00114 rospy.loginfo("Switch: %s is ready", self._param.getSwitchName(switchNum)) 00115 00116 def createIMU(self): 00117 if self._param.isImuInit(): 00118 rospy.loginfo("Configuring IMU: %s", self._param.getIMUName()) 00119 imu = RiCIMU(self._param, self._output) 00120 self._allDevs['imu'].append(imu) 00121 self._output.writeAndWaitForAck(IMUParamResponse(self._param).dataTosend(), 0) 00122 rospy.loginfo("IMU: %s is ready", self._param.getIMUName()) 00123 00124 def createRelays(self): 00125 relayAmount = self._param.getRelaysSize() 00126 for relayNum in xrange(relayAmount): 00127 rospy.loginfo("Configuring relay: %s", self._param.getRelayName(relayNum)) 00128 relay = RiCRelay(self._param, relayNum, self._output) 00129 self._output.writeAndWaitForAck(RelayParamResponse(relayNum, self._param).dataTosend(), relayNum) 00130 self._allDevs['relay'].append(relay) 00131 rospy.loginfo("Relay: %s is ready", self._param.getRelayName(relayNum)) 00132 00133 def createGPS(self): 00134 if self._param.isGpsInit(): 00135 rospy.loginfo("Configuring GPS: %s", self._param.getGpsName()) 00136 gps = RiCGPS(self._param, self._output) 00137 self._output.writeAndWaitForAck(GPSParamResponse(self._param).dataTosend(), 0) 00138 self._allDevs['gps'].append(gps) 00139 rospy.loginfo("GPS: %s is ready", self._param.getGpsName()) 00140 00141 def createPPM(self): 00142 if self._param.isPPMInit(): 00143 rospy.loginfo("Configuring PPM: %s", self._param.getPPMName()) 00144 ppm = RiCPPM(self._param, self._output) 00145 self._output.writeAndWaitForAck(PPMParamResponse(self._param).dataTosend(), 0) 00146 self._allDevs['ppm'].append(ppm) 00147 rospy.loginfo("PPM: %s is ready", self._param.getPPMName()) 00148 00149 def createOpenLoopMotors(self): 00150 motorsAmout = self._param.getOpenLoopNum() 00151 for motorId in xrange(motorsAmout): 00152 rospy.loginfo("Configuring motor: %s", self._param.getOpenLoopName(motorId)) 00153 motor = OpenLoopMotor(motorId, self._param, self._output) 00154 self._allDevs['motorsOl'].append(motor) 00155 self._output.writeAndWaitForAck(OpenLoopMotorParamResponse(motorId,self._param).dataTosend(), motorId) 00156 rospy.loginfo("Motor: %s is ready", self._param.getOpenLoopName(motorId)) 00157 00158 def createBattery(self): 00159 if self._param.isBatteryInit(): 00160 rospy.loginfo("Configuring battery: %s", self._param.getBatteryName()) 00161 battery = RiCBattery(self._param,self._output) 00162 self._allDevs['battery'].append(battery) 00163 self._output.writeAndWaitForAck(BatteryParamResponse(self._param).dataTosend(), 0) 00164 rospy.loginfo("Battery: %s is ready", self._param.getBatteryName()) 00165 00166 def createOpenDiff(self): 00167 if self._param.isInitOpenDiff(): 00168 rospy.loginfo("Configuring differential drive: %s", self._param.getCloseDiffName()) 00169 motorL = self._allDevs['motorsOl'][self._param.getCloseDiffMotorL()] 00170 motorR = self._allDevs['motorsOl'][self._param.getCloseDiffMotorR()] 00171 diff = RiCOpenDiff(self._param, motorL, motorR) 00172 self._allDevs['diff'].append(diff) 00173 rospy.loginfo("Differential drive: %s is ready", self._param.getCloseDiffName()) 00174 00175 def createEmergencySwitch(self): 00176 size = self._param.EmergencyCount() 00177 for i in xrange(size): 00178 rospy.loginfo("Configuring emergency switch: %s", self._param.getEmergencyName(i)) 00179 self._output.writeAndWaitForAck(EmergencySwitchParamResponse(i, self._param).dataTosend(), i) 00180 rospy.loginfo("Emergency switch is ready: %s", self._param.getEmergencyName(i)) 00181 00182 def getDevs(self): 00183 return self._allDevs 00184 00185 00186 def sendFinishBuilding(self): 00187 self._output.writeAndWaitForAck(FinishBuildingRequest().dataTosend(), ID_REQ) 00188 00189