def __init__(self, posX, posY, posTheta): self.w = 15 self.h = 10 self.heading = 0 self.posTheta = posTheta self.posX = posX self.posY = posY self.targetTheta = 0 self.logicalTheta = 0 self.logicalX = 0 self.logicalY = 0 self.Rw = 9 self.Tr = 240 self.D = 9 self.dT1 = 0 self.dT2 = 0 self.T1 = 0 self.T2 = 0 self.counter = 0 self.leftEncoder = Encoder() self.rightEncoder = Encoder() self.leftRangeSensor = RangeSensor(self, self.h / 2 + 1, 0, -1) self.frontRangeSensor = RangeSensor(self, self.w / 2 + 1, 1, 0) self.rightRangeSensor = RangeSensor(self, self.h / 2 + 1, 0, 1) self.statsWidget = RobotStatsWidget(self)
class Robot(): """ Paul v0.0: posX si posY reprezinta coordonatele de pe plansa. logicX si logicY sunt coordonatele relativ la pozitia de start a robotului; sunt coordonate deduse de robot. """ MaxSpeed = 24 def __init__(self, posX, posY, posTheta): self.w = 15 self.h = 10 self.heading = 0 self.posTheta = posTheta self.posX = posX self.posY = posY self.targetTheta = 0 self.logicalTheta = 0 self.logicalX = 0 self.logicalY = 0 self.Rw = 9 self.Tr = 240 self.D = 9 self.dT1 = 0 self.dT2 = 0 self.T1 = 0 self.T2 = 0 self.counter = 0 self.leftEncoder = Encoder() self.rightEncoder = Encoder() self.leftRangeSensor = RangeSensor(self, self.h / 2 + 1, 0, -1) self.frontRangeSensor = RangeSensor(self, self.w / 2 + 1, 1, 0) self.rightRangeSensor = RangeSensor(self, self.h / 2 + 1, 0, 1) self.statsWidget = RobotStatsWidget(self) def setOrientation(self, posTheta): self.posTheta = posTheta self.statsWidget.setOrientation(self.posTheta) def setTargetDirection(self, targetTheta): self.targetTheta = targetTheta if self.targetTheta > 2 * math.pi: self.targetTheta = self.targetTheta % (2 * math.pi) elif self.targetTheta < 0: self.targetTheta = self.targetTheta % (2 * math.pi) #if self.targetTheta > 2 * math.pi: # self.target = 2 * math.pi # print('targetTheta: %f' % self.targetTheta) #elif self.targetTheta < 0: # self.targetTheta = 0 # print('targetTheta: %f' % self.targetTheta) self.statsWidget.setTargetDirection(self.targetTheta) def move(self): self.leftEncoder.addTicks(self.dT1) self.rightEncoder.addTicks(self.dT2) #print('dT1: ' + str(self.dT1) + ', dT2: ' + str(self.dT2)) b = 9 if (self.dT2 - self.dT1) < 0.00001: dHeading = 0 dRealX = self.dT1 * math.cos(self.heading) dRealY = self.dT1 * math.sin(self.heading) elif (self.dT2 - self.dT1) > 24: dHeading = (self.dT2 - self.dT1) / b dRealX = 0 dRealY = 0 else: R = b / 2 * (self.dT2 + self.dT1) / (self.dT2 - self.dT1) dHeading = (self.dT2 - self.dT1) / b dRealX = R * (math.sin(dHeading + self.heading) - math.sin(self.heading)) dRealY = R * (math.cos(self.heading) - math.cos(dHeading + self.heading)) #TODO: This is wrong dHeading2 = 2 * math.pi * (self.Rw / self.D) * \ (self.dT1 - self.dT2) / self.Tr #TODO: This is wrong dRealX2 = self.Rw * math.cos(self.heading) * \ (self.dT1 + self.dT2) * math.pi / self.Tr dRealY2 = self.Rw * math.sin(self.heading) * \ (self.dT1 + self.dT2) * math.pi / self.Tr #print('dHeading: ' + str(dHeading2) + ', dRealX2: ' + str(dRealX2) + ', dRealY2: ' + str(dRealY2)) #print('dRealTheta: ' + str(dHeading) + ', dRealX: ' + str(dRealX) + ', dRealY: ' + str(dRealY)) if self.checkCollision(self.posX + dRealX2, self.posY + dRealY2) == False: self.heading = self.heading + dHeading2 self.posX = self.posX + dRealX2 self.posY = self.posY + dRealY2 self.setOrientation(self.posTheta) self.setTargetDirection(self.targetTheta - dHeading2) #print('heading: ' + str(self.heading) + ', posX: ' + str(self.posX) + ', posY: ' + str(self.posY)) self.counter += 1 if self.counter == 5: self.nextStep() self.counter = 0 def draw(self, painter): painter.setPen(QtGui.QColor(0xffffff)) painter.setBrush(QtGui.QColor(0x00CC66)) #rect = QtCore.QRect(0, 0, self.w, self.h) #original = QtGui.QPolygon(rect, True) #painter.drawPolygon(original) coords = [] coords.append(QtCore.QPoint(0, 0)) coords.append(QtCore.QPoint(self.w, 0)) coords.append(QtCore.QPoint(self.w, self.h)) coords.append(QtCore.QPoint(0, self.h)) original = QtGui.QPolygon(coords) original.translate(-self.w/2, -self.h/2) transform = QtGui.QTransform().rotateRadians(self.posTheta) #painter.fillRect(self.posX, self.posX, self.w, self.h, QtGui.QColor(0x0ff000)) rotated = transform.map(original) #original.translate(self.posX, self.posY) rotated.translate(self.posX, self.posY) #QtCore.qDebug(str(rotated.point(0))) #QtCore.qDebug(str(rotated.point(1))) #QtCore.qDebug(str(rotated.point(2))) #QtCore.qDebug(str(rotated.point(3))) #painter.drawPolygon(original) painter.setBrush(QtGui.QColor(0x0066CC)) painter.drawPolygon(rotated) self.leftRangeSensor.draw(painter, QtGui.QColor(0x00FF00)) self.frontRangeSensor.draw(painter, QtGui.QColor(0xFF0000)) self.rightRangeSensor.draw(painter, QtGui.QColor(0x0000FF)) def nextStep(self): leftDist = self.leftRangeSensor.getDistance() frontDist = self.frontRangeSensor.getDistance() rightDist = self.rightRangeSensor.getDistance() self.statsWidget.setLeftRangeSensorDistance(leftDist) self.statsWidget.setFrontRangeSensorDistance(frontDist) self.statsWidget.setRightRangeSensorDistance(rightDist) dHeading2 = 2 * math.pi * (self.Rw / self.D) * \ (self.dT1 - self.dT2) / self.Tr # dT = dT1 - dT2 dT = self.targetTheta * self.D * self.Tr / (2 * math.pi * self.Rw) if dT < 5: self.setLeftMotorSpeed(24) self.setRightMotorSpeed(24) else: self.setLeftMotorSpeed(dT) self.setRightMotorSpeed(-dT) self.T1 = 0 self.T2 = 0 def increaseLeftMotorSpeed(self, percent): speed = self.dT1 + (percent / 100.0) * Robot.MaxSpeed #print('speed: %f, dT1: %f, percent: %f, MaxSpeed: %f' % (speed, self.dT1, percent, Robot.MaxSpeed)) self.setLeftMotorSpeed(speed) def increaseRightMotorSpeed(self, percent): speed = self.dT2 + (percent / 100.0) * Robot.MaxSpeed #print('speed: %f, dT2: %f, percent: %f, MaxSpeed: %f' % (speed, self.dT2, percent, Robot.MaxSpeed)) self.setRightMotorSpeed(speed) def setLeftMotorSpeed(self, speed): self.dT1 = speed if speed < - Robot.MaxSpeed: speed = - Robot.MaxSpeed elif speed > Robot.MaxSpeed: speed = Robot.MaxSpeed self.statsWidget.setLeftMotorSpeed(speed) #print('dT1: %f dT2: %f' % (self.dT1, self.dT2)) def setRightMotorSpeed(self, speed): self.dT2 = speed if speed < - Robot.MaxSpeed: speed = - Robot.MaxSpeed elif speed > Robot.MaxSpeed: speed = Robot.MaxSpeed self.statsWidget.setRightMotorSpeed(speed) #print('dT1: %f dT2: %f' % (self.dT1, self.dT2)) ''' O verificare de coliziune rudimentara ''' def checkCollision(self, newPosX, newPosY): r = max(self.w, self.h) / 2 x1 = newPosX - r - 1 y1 = newPosY - r - 1 x2 = newPosX + r + 1 y2 = newPosX + r + 1 if x1 < 0 or y1 < 0 or\ x2 >= ImageMap.image.width() or\ y2 >= ImageMap.image.height(): return True if ImageMap.image.pixel(x1, y1) != 0xFFFFFFFF: return True if ImageMap.image.pixel(x1, y2) != 0xFFFFFFFF: return True if ImageMap.image.pixel(x2, y1) != 0xFFFFFFFF: return True if ImageMap.image.pixel(x2, y2) != 0xFFFFFFFF: return True return False def getStatsWidget(self): return self.statsWidget