00001 __author__ = 'tom1231' 00002 from PyQt4.QtGui import * 00003 from BAL.Interface.DeviceFrame import DeviceFrame, CLOSE_LOP_ONE 00004 00005 00006 class CloseLoop(DeviceFrame): 00007 closeLoop = 0 00008 00009 def __init__(self, frame, data, encoders): 00010 DeviceFrame.__init__(self, CLOSE_LOP_ONE, frame, data) 00011 self.mainPorts = encoders 00012 00013 self._pubHz = '20' 00014 self._name = 'motor' 00015 self._lpfHz = '50' 00016 self._lpfAlpha = '0.7' 00017 self._driverAdd = '128' 00018 self._channel = '1' 00019 self._pidHz = '1000' 00020 self._kp = '3.0' 00021 self._ki = '3.0' 00022 self._kd = '0' 00023 self._limit = '1.0' 00024 self._maxSpeed = '16.0' 00025 self._ppr = '4480' 00026 self._timeout = '1000' 00027 self.mainPorts.setCurrentIndex(0) 00028 self._encoder = str(self.mainPorts.currentText()) 00029 self._motorType = '1' 00030 self._dirMotor = False 00031 self._dirEnc = False 00032 self._driverType = '1' 00033 00034 def fromDict(self, data): 00035 self._pubHz = data['pubHz'] 00036 self._name = data['name'] 00037 self._lpfHz = data['lpfHz'] 00038 self._lpfAlpha = data['lpfAlpha'] 00039 self._driverAdd = data['driverAdd'] 00040 self._channel = data['channel'] 00041 self._pidHz = data['pidHz'] 00042 self._kp = data['kp'] 00043 self._ki = data['ki'] 00044 self._kd = data['kd'] 00045 self._limit = data['limit'] 00046 self._maxSpeed = data['maxSpeed'] 00047 self._ppr = data['ppr'] 00048 self._timeout = data['timeout'] 00049 self._encoder = data['encoder'] 00050 self._motorType = data['motorType'] 00051 self._dirMotor = data['dirMotor'] 00052 self._dirEnc = data['dirEnc'] 00053 self._driverType = data['driverType'] 00054 00055 def toDict(self): 00056 data = dict() 00057 data['type'] = CLOSE_LOP_ONE 00058 data['pubHz'] = self._pubHz 00059 data['name'] = self._name 00060 data['lpfHz'] = self._lpfHz 00061 data['lpfAlpha'] = self._lpfAlpha 00062 data['driverAdd'] = self._driverAdd 00063 data['channel'] = self._channel 00064 data['pidHz'] = self._pidHz 00065 data['kp'] = self._kp 00066 data['ki'] = self._ki 00067 data['kd'] = self._kd 00068 data['limit'] = self._limit 00069 data['maxSpeed'] = self._maxSpeed 00070 data['ppr'] = self._ppr 00071 data['timeout'] = self._timeout 00072 data['encoder'] = self._encoder 00073 data['motorType'] = self._motorType 00074 data['dirMotor'] = self._dirMotor 00075 data['dirEnc'] = self._dirEnc 00076 data['driverType'] = self._driverType 00077 00078 return data 00079 00080 def getTypeIndex(self): 00081 if self._motorType == '1': return 1 00082 return 0 00083 00084 def getDriverTypeIndex(self): 00085 if self._driverType == '1': return 0 00086 return 1 00087 00088 def findItem(self): 00089 for i in xrange(self.mainPorts.count()): 00090 if self._encoder == str(self.mainPorts.itemText(i)): 00091 return i 00092 return 1 00093 00094 def getEncoder(self): 00095 return self._encoder 00096 00097 def add(self): 00098 old = self._name 00099 self._name = str(self.name.text()) 00100 00101 if not self.nameIsValid(): 00102 error = QErrorMessage() 00103 error.setWindowTitle("Same name error") 00104 error.showMessage("Name already taken.") 00105 error.exec_() 00106 self._name = old 00107 self._isValid = False 00108 return 00109 00110 self._isValid = True 00111 self._dirEnc = self.dirEnc.isChecked() 00112 self._dirMotor = self.dirMotor.isChecked() 00113 self._pubHz = str(self.pubHz.text()) 00114 self._name = str(self.name.text()) 00115 self._lpfHz = str(self.lpfHz.text()) 00116 self._lpfAlpha = str(self.lpfAlpha.text()) 00117 self._driverAdd = str(self.driverAdd.text()) 00118 self._channel = str(self.channel.text()) 00119 self._encoder = str(self.encoders.currentText()) 00120 self._pidHz = str(self.pidHz.text()) 00121 self._kp = str(self.kp.text()) 00122 self._ki = str(self.ki.text()) 00123 self._kd = str(self.kd.text()) 00124 self._limit = str(self.limit.text()) 00125 self._maxSpeed = str(self.maxSpeed.text()) 00126 self._ppr = str(self.ppr.text()) 00127 self._timeout = str(self.timeout.text()) 00128 self._motorType = str(self.motorTypes.itemData(self.motorTypes.currentIndex()).toString()) 00129 self._driverType = str(self.driverType.itemData(self.driverType.currentIndex()).toString()) 00130 00131 self.mainPorts.removeItem(self.encoders.currentIndex()) 00132 00133 def saveToFile(self, file): 00134 file.write('closeLoop' + str(CloseLoop.closeLoop) + '/publishHz: ' + self._pubHz + '\n') 00135 file.write('closeLoop' + str(CloseLoop.closeLoop) + '/name: ' + self._name + '\n') 00136 file.write('closeLoop' + str(CloseLoop.closeLoop) + '/LPFAlpha: ' + self._lpfAlpha + '\n') 00137 file.write('closeLoop' + str(CloseLoop.closeLoop) + '/LPFHz: ' + self._lpfHz + '\n') 00138 file.write('closeLoop' + str(CloseLoop.closeLoop) + '/driverAddress: ' + self._driverAdd + '\n') 00139 file.write('closeLoop' + str(CloseLoop.closeLoop) + '/channel: ' + self._channel + '\n') 00140 file.write('closeLoop' + str(CloseLoop.closeLoop) + '/port: ' + self._encoder + '\n') 00141 file.write('closeLoop' + str(CloseLoop.closeLoop) + '/PIDHz: ' + self._pidHz + '\n') 00142 file.write('closeLoop' + str(CloseLoop.closeLoop) + '/kP: ' + self._kp + '\n') 00143 file.write('closeLoop' + str(CloseLoop.closeLoop) + '/kI: ' + self._ki + '\n') 00144 file.write('closeLoop' + str(CloseLoop.closeLoop) + '/kD: ' + self._kd + '\n') 00145 file.write('closeLoop' + str(CloseLoop.closeLoop) + '/limit: ' + self._limit + '\n') 00146 file.write('closeLoop' + str(CloseLoop.closeLoop) + '/maxSpeed: ' + self._maxSpeed + '\n') 00147 file.write('closeLoop' + str(CloseLoop.closeLoop) + '/cpr: ' + self._ppr + '\n') 00148 file.write('closeLoop' + str(CloseLoop.closeLoop) + '/timeout: ' + self._timeout + '\n') 00149 file.write('closeLoop' + str(CloseLoop.closeLoop) + '/motorType: ' + self._motorType + '\n') 00150 file.write('closeLoop' + str(CloseLoop.closeLoop) + '/encoderType: ' + self.encoderType() + '\n') 00151 if not self._dirMotor: file.write('closeLoop' + str(CloseLoop.closeLoop) + '/direction: ' + '1' + '\n') 00152 else: file.write('closeLoop' + str(CloseLoop.closeLoop) + '/direction: ' + '-1' + '\n') 00153 if not self._dirEnc: file.write('closeLoop' + str(CloseLoop.closeLoop) + '/directionE: ' + '1' + '\n') 00154 else: file.write('closeLoop' + str(CloseLoop.closeLoop) + '/directionE: ' + '-1' + '\n') 00155 CloseLoop.closeLoop += 1 00156 00157 def getName(self): 00158 return self._name 00159 00160 def printDetails(self): 00161 show = 'Channel: ' 00162 if self._driverType == '2': show = 'Pin: ' 00163 self._frame.layout().addRow(QLabel('Publish Hz: '), QLabel(self._pubHz)) 00164 self._frame.layout().addRow(QLabel('name: '), QLabel(self._name)) 00165 self._frame.layout().addRow(QLabel('LPF HZ: '), QLabel(self._lpfHz)) 00166 self._frame.layout().addRow(QLabel('LPF Alpha: '), QLabel(self._lpfAlpha)) 00167 if self._driverType == '1': 00168 self._frame.layout().addRow(QLabel('Driver Address: '), QLabel(self._driverAdd)) 00169 self._frame.layout().addRow(QLabel(show), QLabel(self._channel)) 00170 self._frame.layout().addRow(QLabel('PID Hz: '), QLabel(self._pidHz)) 00171 self._frame.layout().addRow(QLabel('Kp parameter: '), QLabel(self._kp)) 00172 self._frame.layout().addRow(QLabel('Ki parameter: '), QLabel(self._ki)) 00173 self._frame.layout().addRow(QLabel('Kd parameter: '), QLabel(self._kd)) 00174 self._frame.layout().addRow(QLabel('Integral limit: '), QLabel(self._limit)) 00175 self._frame.layout().addRow(QLabel('Max speed: '), QLabel(self._maxSpeed)) 00176 self._frame.layout().addRow(QLabel('PPR parameter: '), QLabel(self._ppr)) 00177 self._frame.layout().addRow(QLabel('Timeout (in milliSecond): '), QLabel(self._timeout)) 00178 self.printEncoder() 00179 if self._motorType == '1': self._frame.layout().addRow(QLabel('Type: '), QLabel('Speed')) 00180 else: self._frame.layout().addRow(QLabel('Type: '), QLabel('Position')) 00181 if self._dirMotor: self._frame.layout().addRow(QLabel('Reverse motor direction: '), QLabel('Enable')) 00182 else: self._frame.layout().addRow(QLabel('Reverse motor direction: '), QLabel('Disable')) 00183 if self._dirEnc: self._frame.layout().addRow(QLabel('Reverse encoder direction: '), QLabel('Enable')) 00184 else: self._frame.layout().addRow(QLabel('Reverse encoder direction: '), QLabel('Disable')) 00185 00186 def showDetails(self, items=None): 00187 self.motorTypes = QComboBox() 00188 self.driverType = QComboBox() 00189 00190 self.motorTypes.addItem('Position', '0') 00191 self.motorTypes.addItem('Speed', '1') 00192 00193 self.driverType.addItem('Sabertooth Serial', '1') 00194 self.driverType.addItem('RC Servo', '2') 00195 00196 self.driverType.setCurrentIndex(self.getDriverTypeIndex()) 00197 self.motorTypes.setCurrentIndex(self.getTypeIndex()) 00198 00199 self.driverType.currentIndexChanged.connect(self.driverTypeChangeEven) 00200 00201 self.pubHz = QLineEdit(self._pubHz) 00202 self.name = QLineEdit(self._name) 00203 self.lpfHz = QLineEdit(self._lpfHz) 00204 self.lpfAlpha = QLineEdit(self._lpfAlpha) 00205 self.driverAdd = QLineEdit(self._driverAdd) 00206 self.channel = QLineEdit(self._channel) 00207 self.pidHz = QLineEdit(self._pidHz) 00208 self.kp = QLineEdit(self._kp) 00209 self.ki = QLineEdit(self._ki) 00210 self.kd = QLineEdit(self._kd) 00211 self.limit = QLineEdit(self._limit) 00212 self.maxSpeed = QLineEdit(self._maxSpeed) 00213 self.ppr = QLineEdit(self._ppr) 00214 self.timeout = QLineEdit(self._timeout) 00215 self.dirMotor = QCheckBox('') 00216 self.dirEnc = QCheckBox('') 00217 if self._driverType == '1': self.channelLabel = QLabel('Channel: ') 00218 else: self.channelLabel = QLabel('Pin: ') 00219 00220 self.dirMotor.setChecked(self._dirMotor) 00221 self.dirEnc.setChecked(self._dirEnc) 00222 00223 self.driverTypeChangeEven(self.driverType.currentIndex()) 00224 00225 self._frame.layout().addRow(QLabel('Publish Hz: '), self.pubHz) 00226 self._frame.layout().addRow(QLabel('name: '), self.name) 00227 self._frame.layout().addRow(QLabel('LPF HZ: '), self.lpfHz) 00228 self._frame.layout().addRow(QLabel('LPF Alpha: '), self.lpfAlpha) 00229 self._frame.layout().addRow(QLabel('Driver Address: '), self.driverAdd) 00230 self._frame.layout().addRow(self.channelLabel, self.channel) 00231 self._frame.layout().addRow(QLabel('PID Hz: '), self.pidHz) 00232 self._frame.layout().addRow(QLabel('Kp parameter: '), self.kp) 00233 self._frame.layout().addRow(QLabel('Ki parameter: '), self.ki) 00234 self._frame.layout().addRow(QLabel('Kd parameter: '), self.kd) 00235 self._frame.layout().addRow(QLabel('Integral limit: '), self.limit) 00236 self._frame.layout().addRow(QLabel('Max speed: '), self.maxSpeed) 00237 self._frame.layout().addRow(QLabel('PPR parameter: '), self.ppr) 00238 self._frame.layout().addRow(QLabel('Timeout (in milliSecond): '), self.timeout) 00239 self.setEncode() 00240 self._frame.layout().addRow(QLabel('Type: '), self.motorTypes) 00241 self._frame.layout().addRow(QLabel('Reverse motor direction: '), self.dirMotor) 00242 self._frame.layout().addRow(QLabel('Reverse encoder direction: '), self.dirEnc) 00243 self._frame.layout().addRow(QLabel('Type: '), self.driverType) 00244 00245 def setEncode(self): 00246 self.encoders = QComboBox() 00247 self.encoders.addItems([self.mainPorts.itemText(i) for i in xrange(self.mainPorts.count())]) 00248 self.encoders.setCurrentIndex(self.findItem()) 00249 00250 self._frame.layout().addRow(QLabel('Encoder: '), self.encoders) 00251 00252 def printEncoder(self): 00253 self._frame.layout().addRow(QLabel('Encoder: '), QLabel(self._encoder)) 00254 00255 def encoderType(self): 00256 return '1' 00257 00258 def driverTypeChangeEven(self, index): 00259 if index == 0: 00260 self.driverAdd.setEnabled(True) 00261 self.driverAdd.setEchoMode(QLineEdit.Normal) 00262 self.driverAdd.setText('128') 00263 self.channelLabel.setText('Channel: ') 00264 return 00265 self.driverAdd.setEnabled(False) 00266 self.driverAdd.setEchoMode(QLineEdit.NoEcho) 00267 self.driverAdd.setText('999') 00268 self.channelLabel.setText('Pin: ') 00269