def __init__(self, endpoint, modules):
QWidget.__init__(self)
self.ic = Ice.initialize(sys.argv)
self.mods = modules
self.prx = self.ic.stringToProxy(endpoint)
self.proxy = self.mods['RoboCompSmar'].SmarPrx.checkedCast(self.prx)
self.ui = Ui_windowSmarTest()
self.ui.setupUi(self)
self.show()
print "AQUIIIIIIIIIIIIIIIIIIII"
#~ print float('inf')
#~ #TEST ZONE
#~ self.testWidget = QGroupBox(self)
#~ self.testWidget.resize(250, 240)
#~ self.testWidget.move(0, 0)
#~ self.testWidget.setTitle('Smar Test Panel')
#~ self.testWidget.show()
#~
#~
#~ self.turnRight90Button = QPushButton("Turn right 90", self)
#~ self.turnRight90Button.show()
#~ self.turnRight90Button.move(0, 20)
#~ self.connect(self.turnRight90Button, SIGNAL('clicked()'), self.right90Clicked)
#~ self.turnRight90Button.resize(250, 40)
#~
#~
#~ self.turnRadiansButton = QPushButton("Turn radians...", self)
#~ self.turnRadiansButton.show()
#~ self.turnRadiansButton.move(0,self.turnRight90Button.y()+self.turnRight90Button.height())
#~ self.connect(self.turnRadiansButton, SIGNAL('clicked()'), self.turnRadiansClicked)
#~ self.turnRadiansButton.resize(250, 40)
#~
#~ self.angleLabel = QLabel(self)
#~ self.angleLabel.setText("Angle (radians):")
#~ self.angleLabel.resize(120, 70)
#~ self.angleLabel.move(self.turnRight90Button.x()+self.turnRight90Button.width(),self.turnRight90Button.y())
#~ self.angleLabel.show()
#~ self.angleSpinbox = QDoubleSpinBox(self)
#~ self.angleSpinbox.show()
#~ self.angleSpinbox.move(self.turnRadiansButton.x()+self.turnRadiansButton.width(),self.turnRadiansButton.y())
#~ self.angleSpinbox.resize(250, 40)
#~ self.angleSpinbox.setMaximum(3.14)
#~ self.angleSpinbox.setMinimum(-3.14)
#~ self.angleSpinbox.setSingleStep(0.1)
#~
#~
#~
#~
#~
#~
#~ self.rightSquareButton = QPushButton("Right Square", self)
#~ self.rightSquareButton.show()
#~ self.rightSquareButton.move(0,self.turnRadiansButton.y()+self.turnRadiansButton.height())
#~ self.connect(self.rightSquareButton, SIGNAL('clicked()'), self.rightSquareClicked)
#~ self.rightSquareButton.resize(250, 40)
#~
#~ self.leftSquareButton = QPushButton("Left Square", self)
#~ self.leftSquareButton.show()
#~ self.leftSquareButton.move(0,self.rightSquareButton.y()+self.rightSquareButton.height())
#~ self.connect(self.leftSquareButton, SIGNAL('clicked()'), self.leftSquareClicked)
#~ self.leftSquareButton.resize(250, 40)
#~
#~
#~ self.distanceLabel = QLabel(self)
#~ self.distanceLabel.setText("Distance (mm):")
#~ self.distanceLabel.resize(120, 70)
#~ self.distanceLabel.move(self.rightSquareButton.x()+self.rightSquareButton.width(),self.rightSquareButton.y())
#~ self.distanceLabel.show()
#~ self.distanceSpinbox = QDoubleSpinBox(self)
#~ self.distanceSpinbox.show()
#~ self.distanceSpinbox.move(self.leftSquareButton.x()+self.leftSquareButton.width(),self.leftSquareButton.y())
#~ self.distanceSpinbox.resize(250, 40)
#~ self.distanceSpinbox.setMaximum(5000.)
#~ self.distanceSpinbox.setMinimum(-5000.)
#~ self.distanceSpinbox.setSingleStep(10)
#~ self.advanceButton = QPushButton("Advance...", self)
#~ self.advanceButton.show()
#~ self.advanceButton.move(0,self.leftSquareButton.y()+self.leftSquareButton.height())
#~ self.connect(self.advanceButton, SIGNAL('clicked()'), self.moveForward)
#~ self.advanceButton.resize(250, 40)
self.connect(self.ui.advancebutton, SIGNAL('clicked()'), self.moveForward)
self.connect(self.ui.turn90rightButton, SIGNAL('clicked()'), self.right90Clicked)
self.connect(self.ui.turn90leftButton, SIGNAL('clicked()'), self.left90Clicked)
self.connect(self.ui.turnRadiansButton, SIGNAL('clicked()'), self.turnRadiansClicked)
self.connect(self.ui.rightSquareButton, SIGNAL('clicked()'), self.rightSquareClicked)
self.connect(self.ui.leftSquareButton, SIGNAL('clicked()'), self.leftSquareClicked)
self.job()
def __init__(self, endpoint, modules):
QWidget.__init__(self)
self.ic = Ice.initialize(sys.argv)
self.mods = modules
self.prx = self.ic.stringToProxy(endpoint)
self.proxy = self.mods['RoboCompSmar'].SmarPrx.checkedCast(self.prx)
self.ui = Ui_windowSmarTest()
self.ui.setupUi(self)
self.show()
print "AQUIIIIIIIIIIIIIIIIIIII"
#~ print float('inf')
#~ #TEST ZONE
#~ self.testWidget = QGroupBox(self)
#~ self.testWidget.resize(250, 240)
#~ self.testWidget.move(0, 0)
#~ self.testWidget.setTitle('Smar Test Panel')
#~ self.testWidget.show()
#~
#~
#~ self.turnRight90Button = QPushButton("Turn right 90", self)
#~ self.turnRight90Button.show()
#~ self.turnRight90Button.move(0, 20)
#~ self.connect(self.turnRight90Button, SIGNAL('clicked()'), self.right90Clicked)
#~ self.turnRight90Button.resize(250, 40)
#~
#~
#~ self.turnRadiansButton = QPushButton("Turn radians...", self)
#~ self.turnRadiansButton.show()
#~ self.turnRadiansButton.move(0,self.turnRight90Button.y()+self.turnRight90Button.height())
#~ self.connect(self.turnRadiansButton, SIGNAL('clicked()'), self.turnRadiansClicked)
#~ self.turnRadiansButton.resize(250, 40)
#~
#~ self.angleLabel = QLabel(self)
#~ self.angleLabel.setText("Angle (radians):")
#~ self.angleLabel.resize(120, 70)
#~ self.angleLabel.move(self.turnRight90Button.x()+self.turnRight90Button.width(),self.turnRight90Button.y())
#~ self.angleLabel.show()
#~ self.angleSpinbox = QDoubleSpinBox(self)
#~ self.angleSpinbox.show()
#~ self.angleSpinbox.move(self.turnRadiansButton.x()+self.turnRadiansButton.width(),self.turnRadiansButton.y())
#~ self.angleSpinbox.resize(250, 40)
#~ self.angleSpinbox.setMaximum(3.14)
#~ self.angleSpinbox.setMinimum(-3.14)
#~ self.angleSpinbox.setSingleStep(0.1)
#~
#~
#~
#~
#~
#~
#~ self.rightSquareButton = QPushButton("Right Square", self)
#~ self.rightSquareButton.show()
#~ self.rightSquareButton.move(0,self.turnRadiansButton.y()+self.turnRadiansButton.height())
#~ self.connect(self.rightSquareButton, SIGNAL('clicked()'), self.rightSquareClicked)
#~ self.rightSquareButton.resize(250, 40)
#~
#~ self.leftSquareButton = QPushButton("Left Square", self)
#~ self.leftSquareButton.show()
#~ self.leftSquareButton.move(0,self.rightSquareButton.y()+self.rightSquareButton.height())
#~ self.connect(self.leftSquareButton, SIGNAL('clicked()'), self.leftSquareClicked)
#~ self.leftSquareButton.resize(250, 40)
#~
#~
#~ self.distanceLabel = QLabel(self)
#~ self.distanceLabel.setText("Distance (mm):")
#~ self.distanceLabel.resize(120, 70)
#~ self.distanceLabel.move(self.rightSquareButton.x()+self.rightSquareButton.width(),self.rightSquareButton.y())
#~ self.distanceLabel.show()
#~ self.distanceSpinbox = QDoubleSpinBox(self)
#~ self.distanceSpinbox.show()
#~ self.distanceSpinbox.move(self.leftSquareButton.x()+self.leftSquareButton.width(),self.leftSquareButton.y())
#~ self.distanceSpinbox.resize(250, 40)
#~ self.distanceSpinbox.setMaximum(5000.)
#~ self.distanceSpinbox.setMinimum(-5000.)
#~ self.distanceSpinbox.setSingleStep(10)
#~ self.advanceButton = QPushButton("Advance...", self)
#~ self.advanceButton.show()
#~ self.advanceButton.move(0,self.leftSquareButton.y()+self.leftSquareButton.height())
#~ self.connect(self.advanceButton, SIGNAL('clicked()'), self.moveForward)
#~ self.advanceButton.resize(250, 40)
self.connect(self.ui.advancebutton, SIGNAL('clicked()'),
self.moveForward)
self.connect(self.ui.turn90rightButton, SIGNAL('clicked()'),
self.right90Clicked)
self.connect(self.ui.turn90leftButton, SIGNAL('clicked()'),
self.left90Clicked)
self.connect(self.ui.turnRadiansButton, SIGNAL('clicked()'),
self.turnRadiansClicked)
self.connect(self.ui.rightSquareButton, SIGNAL('clicked()'),
self.rightSquareClicked)
self.connect(self.ui.leftSquareButton, SIGNAL('clicked()'),
self.leftSquareClicked)
self.job()
class C(QWidget):
def __init__(self, endpoint, modules):
QWidget.__init__(self)
self.ic = Ice.initialize(sys.argv)
self.mods = modules
self.prx = self.ic.stringToProxy(endpoint)
self.proxy = self.mods['RoboCompSmar'].SmarPrx.checkedCast(self.prx)
self.ui = Ui_windowSmarTest()
self.ui.setupUi(self)
self.show()
print "AQUIIIIIIIIIIIIIIIIIIII"
#~ print float('inf')
#~ #TEST ZONE
#~ self.testWidget = QGroupBox(self)
#~ self.testWidget.resize(250, 240)
#~ self.testWidget.move(0, 0)
#~ self.testWidget.setTitle('Smar Test Panel')
#~ self.testWidget.show()
#~
#~
#~ self.turnRight90Button = QPushButton("Turn right 90", self)
#~ self.turnRight90Button.show()
#~ self.turnRight90Button.move(0, 20)
#~ self.connect(self.turnRight90Button, SIGNAL('clicked()'), self.right90Clicked)
#~ self.turnRight90Button.resize(250, 40)
#~
#~
#~ self.turnRadiansButton = QPushButton("Turn radians...", self)
#~ self.turnRadiansButton.show()
#~ self.turnRadiansButton.move(0,self.turnRight90Button.y()+self.turnRight90Button.height())
#~ self.connect(self.turnRadiansButton, SIGNAL('clicked()'), self.turnRadiansClicked)
#~ self.turnRadiansButton.resize(250, 40)
#~
#~ self.angleLabel = QLabel(self)
#~ self.angleLabel.setText("Angle (radians):")
#~ self.angleLabel.resize(120, 70)
#~ self.angleLabel.move(self.turnRight90Button.x()+self.turnRight90Button.width(),self.turnRight90Button.y())
#~ self.angleLabel.show()
#~ self.angleSpinbox = QDoubleSpinBox(self)
#~ self.angleSpinbox.show()
#~ self.angleSpinbox.move(self.turnRadiansButton.x()+self.turnRadiansButton.width(),self.turnRadiansButton.y())
#~ self.angleSpinbox.resize(250, 40)
#~ self.angleSpinbox.setMaximum(3.14)
#~ self.angleSpinbox.setMinimum(-3.14)
#~ self.angleSpinbox.setSingleStep(0.1)
#~
#~
#~
#~
#~
#~
#~ self.rightSquareButton = QPushButton("Right Square", self)
#~ self.rightSquareButton.show()
#~ self.rightSquareButton.move(0,self.turnRadiansButton.y()+self.turnRadiansButton.height())
#~ self.connect(self.rightSquareButton, SIGNAL('clicked()'), self.rightSquareClicked)
#~ self.rightSquareButton.resize(250, 40)
#~
#~ self.leftSquareButton = QPushButton("Left Square", self)
#~ self.leftSquareButton.show()
#~ self.leftSquareButton.move(0,self.rightSquareButton.y()+self.rightSquareButton.height())
#~ self.connect(self.leftSquareButton, SIGNAL('clicked()'), self.leftSquareClicked)
#~ self.leftSquareButton.resize(250, 40)
#~
#~
#~ self.distanceLabel = QLabel(self)
#~ self.distanceLabel.setText("Distance (mm):")
#~ self.distanceLabel.resize(120, 70)
#~ self.distanceLabel.move(self.rightSquareButton.x()+self.rightSquareButton.width(),self.rightSquareButton.y())
#~ self.distanceLabel.show()
#~ self.distanceSpinbox = QDoubleSpinBox(self)
#~ self.distanceSpinbox.show()
#~ self.distanceSpinbox.move(self.leftSquareButton.x()+self.leftSquareButton.width(),self.leftSquareButton.y())
#~ self.distanceSpinbox.resize(250, 40)
#~ self.distanceSpinbox.setMaximum(5000.)
#~ self.distanceSpinbox.setMinimum(-5000.)
#~ self.distanceSpinbox.setSingleStep(10)
#~ self.advanceButton = QPushButton("Advance...", self)
#~ self.advanceButton.show()
#~ self.advanceButton.move(0,self.leftSquareButton.y()+self.leftSquareButton.height())
#~ self.connect(self.advanceButton, SIGNAL('clicked()'), self.moveForward)
#~ self.advanceButton.resize(250, 40)
self.connect(self.ui.advancebutton, SIGNAL('clicked()'), self.moveForward)
self.connect(self.ui.turn90rightButton, SIGNAL('clicked()'), self.right90Clicked)
self.connect(self.ui.turn90leftButton, SIGNAL('clicked()'), self.left90Clicked)
self.connect(self.ui.turnRadiansButton, SIGNAL('clicked()'), self.turnRadiansClicked)
self.connect(self.ui.rightSquareButton, SIGNAL('clicked()'), self.rightSquareClicked)
self.connect(self.ui.leftSquareButton, SIGNAL('clicked()'), self.leftSquareClicked)
self.job()
def job(self):
self.current_state = self.mods['RoboCompSmar'].TBaseState()
self.current_state = self.proxy.getBaseState()
self.ui.SmarPositionLabel.setText(QString("X: "+str(self.current_state.x)+" Y: "+str(self.current_state.z)+" Yaw: "+str(self.current_state.alpha)))
#~ print "job"
#while(True):
#self.proxy.setPosition(goal1)
#time.sleep(6)
#self.proxy.setPosition(goal2)
#time.sleep(6)
#~ try:
#~ self.states = self.proxy.getAllMotorState()
#~ self.states = 0
#~ self.refused = 0
#~ state = self.states[str(self.combo.currentText())]
#~ self.readLabel.setText(QString.number(state.pos))
#~ except Ice.ConnectionRefusedException:
#~ if self.refused == 0:
#~ self.refused = 1
#~ print 'Connection refused'
#~ elif self.refused == 1:
#~ self.refused == 2
#self.clicked()
#~ TODO: CONTROLAR EXCEPCION DE GIRO SOBRE 0
def right90Clicked(self):
print "right90Clicked"
print "with radius of",self.ui.displaceVAlue.value(),"m and Velocity:",self.ui.speedValue.value()
#~ worker->computeAPJE(radius, vel);
#get base state
self.old_state = self.mods['RoboCompSmar'].TBaseState()
self.new_state = self.mods['RoboCompSmar'].TBaseState()
self.old_state = self.proxy.getBaseState()
radius = math.fabs(self.ui.radiusValue.value())
base_vel = self.ui.speedValue.value()
self.proxy.setSpeedBase(0.0, radius);
#turn right
time.sleep(3)
self.proxy.setSpeedBase(base_vel, radius);
turning = True;
reduced = False;
#Wait until 90 turned
while(turning):
self.new_state = self.proxy.getBaseState()
#~ print "old"+str(self.old_state.alpha)
#~ print "new"+str(self.new_state.alpha)
distance = math.fabs(self.new_state.alpha-self.old_state.alpha)
if(distance>=(math.pi/2)):
turning = False
print math.fabs(self.new_state.alpha-self.old_state.alpha)
elif(distance>=1.2):
d = math.fabs(math.pi/2. - distance)
print d
new_velocity = self.calculateVelocity(base_vel, math.fabs(1.2-math.pi/2.), d)
print "new velocity ", new_velocity
if(new_velocity>0.02):
self.proxy.setSpeedBase(new_velocity, radius)
time.sleep(0.1)
self.proxy.setSpeedBase(0.0, radius);
time.sleep(3)
self.proxy.setSpeedBase(0.0, float('inf'))
#~ TODO: CONTROLAR EXCEPCION DE GIRO SOBRE 0
def left90Clicked(self):
print "left90Clicked"
print "with radius of",self.ui.radiusValue.value(),"m and Velocity:",self.ui.speedValue.value()
self.old_state = self.mods['RoboCompSmar'].TBaseState()
self.new_state = self.mods['RoboCompSmar'].TBaseState()
self.old_state = self.proxy.getBaseState()
radius = -math.fabs(self.ui.radiusValue.value())
base_vel = self.ui.speedValue.value()
if radius == 0:
base_vel = -math.fabs(base_vel)
self.proxy.setSpeedBase(0.0, -math.fabs(radius));
#turn right
time.sleep(3)
self.proxy.setSpeedBase(base_vel, -math.fabs(radius));
turning = True;
reduced = False;
#Wait until 90 turned
while(turning):
self.new_state = self.proxy.getBaseState()
#~ print "old"+str(self.old_state.alpha)
#~ print "new"+str(self.new_state.alpha)
distance = math.fabs(self.new_state.alpha-self.old_state.alpha)
if(distance>=(math.pi/2)):
turning = False
print math.fabs(self.new_state.alpha-self.old_state.alpha)
elif(distance>=1.2):
d = math.fabs(math.pi/2. - distance)
print d
new_velocity = self.calculateVelocity(base_vel, math.fabs(1.2-math.pi/2.), d)
if radius == 0:
new_velocity = -math.fabs(new_velocity)
print "new velocity ", new_velocity
if(new_velocity>0.02):
self.proxy.setSpeedBase(new_velocity, -math.fabs(radius))
time.sleep(0.1)
self.proxy.setSpeedBase(0.0, -math.fabs(radius));
time.sleep(3)
self.proxy.setSpeedBase(0.0, float('inf'))
def calculateVelocity(self, base_vel, cut_pos, distance):
# print str(base_vel)+" - (("+str(base_vel)+"/"+str(cut_pos)+")*("+str(distance)+"-"+str(cut_pos)+"))"
# return base_vel - ((base_vel/cut_pos)*(distance-cut_pos))
# f = satura(K*disancia + C, Vm)
vel = (base_vel/cut_pos) * math.fabs(distance) + 0.02
if distance < 0: vel = -vel
if math.fabs(vel) > math.fabs(base_vel):
vel = base_vel
return vel
def waitRadians(self, radians):
pass
def waitAdvance(self, milimeters):
pass
def moveForward(self):
print "moveForward"
print "with avance of",self.ui.displaceVAlue.value(),"mm and velocity",self.ui.speedValue.value()
self.old_state = self.mods['RoboCompSmar'].TBaseState()
self.new_state = self.mods['RoboCompSmar'].TBaseState()
self.old_state = self.proxy.getBaseState()
base_vel = self.ui.speedValue.value()
goal_dist = self.ui.displaceVAlue.value()
if goal_dist > 0:
self.proxy.setSpeedBase(base_vel, float('inf'))
else:
self.proxy.setSpeedBase(base_vel, float('-inf'));
advancing = True;
while(advancing):
self.new_state = self.proxy.getBaseState()
xx=self.new_state.x-self.old_state.x
yy=self.new_state.z-self.old_state.z
actual_distance = math.sqrt(math.pow(xx,2)+math.pow(yy,2))
print "Actual distance: "+str(actual_distance), self.new_state.alpha - self.old_state.alpha
if(actual_distance>=math.fabs(goal_dist)):
advancing = False
self.proxy.setSpeedBase(0.0, float('inf'))
else:
base_vel2 = base_vel
if math.fabs(actual_distance-math.fabs(goal_dist)) < 1000.:
base_vel2 = 0.3
elif math.fabs(actual_distance-math.fabs(goal_dist)) < 500.:
base_vel2 = 0.2
if math.fabs(actual_distance-math.fabs(goal_dist)) < 300.:
base_vel2 = 0.1
if goal_dist > 0:
self.proxy.setSpeedBase(base_vel2, float('inf'))
else:
self.proxy.setSpeedBase(base_vel2, float('-inf'));
time.sleep(0.1)
def rightSquareClicked(self):
print "rightSquareClicked"
print "with radius",self.ui.radiusValue.value(),"and velocity",self.ui.speedValue.value()
self.moveForward()
self.right90Clicked()
time.sleep(1.8)
self.moveForward()
self.right90Clicked()
time.sleep(1.8)
self.moveForward()
self.right90Clicked()
time.sleep(1.8)
self.moveForward()
self.right90Clicked()
def leftSquareClicked(self):
print "leftSquareClicked"
print "with radius",self.ui.radiusValue.value(),"and velocity",self.ui.speedValue.value()
self.moveForward()
self.left90Clicked()
time.sleep(1.8)
self.moveForward()
self.left90Clicked()
time.sleep(1.8)
self.moveForward()
self.left90Clicked()
time.sleep(1.8)
self.moveForward()
self.left90Clicked()
#TODO: COMPROBAR QUE LA VELOCIDAD NO ES 0
def turnRadiansClicked(self):
#~ print "NOT IMPLEMENTE YET"
print "turnRadiansClicked"
print self.ui.angDisplaceValue.value(),"radians with radius",self.ui.radiusValue.value(),"and velocity",self.ui.speedValue.value()
self.old_state = self.mods['RoboCompSmar'].TBaseState()
self.new_state = self.mods['RoboCompSmar'].TBaseState()
self.old_state = self.proxy.getBaseState()
radius = self.ui.radiusValue.value()
base_vel = self.ui.speedValue.value()
radians = self.ui.angDisplaceValue.value()
self.proxy.setSpeedBase(0.0,radius)
#turn right
time.sleep(3)
self.proxy.setSpeedBase(base_vel,radius);
turning = True;
reduced = False;
#Wait until 90 turned
while(turning):
self.new_state = self.proxy.getBaseState()
distance = math.fabs(self.new_state.alpha-self.old_state.alpha)
d = math.fabs(radians - distance)
print "Actual alpha: ", self.new_state.alpha - self.old_state.alpha
if(distance>=radians):
turning = False
print math.fabs(self.new_state.alpha-self.old_state.alpha)
elif(d<0.3):
print d
new_velocity = self.calculateVelocity(base_vel, 0.3, d)
print "new velocity ", new_velocity
if(new_velocity>0.02):
self.proxy.setSpeedBase(new_velocity,radius)
time.sleep(0.1)
self.proxy.setSpeedBase(0.0,radius);
time.sleep(3)
self.proxy.setSpeedBase(0.0, float('inf'))
class C(QWidget):
def __init__(self, endpoint, modules):
QWidget.__init__(self)
self.ic = Ice.initialize(sys.argv)
self.mods = modules
self.prx = self.ic.stringToProxy(endpoint)
self.proxy = self.mods['RoboCompSmar'].SmarPrx.checkedCast(self.prx)
self.ui = Ui_windowSmarTest()
self.ui.setupUi(self)
self.show()
print "AQUIIIIIIIIIIIIIIIIIIII"
#~ print float('inf')
#~ #TEST ZONE
#~ self.testWidget = QGroupBox(self)
#~ self.testWidget.resize(250, 240)
#~ self.testWidget.move(0, 0)
#~ self.testWidget.setTitle('Smar Test Panel')
#~ self.testWidget.show()
#~
#~
#~ self.turnRight90Button = QPushButton("Turn right 90", self)
#~ self.turnRight90Button.show()
#~ self.turnRight90Button.move(0, 20)
#~ self.connect(self.turnRight90Button, SIGNAL('clicked()'), self.right90Clicked)
#~ self.turnRight90Button.resize(250, 40)
#~
#~
#~ self.turnRadiansButton = QPushButton("Turn radians...", self)
#~ self.turnRadiansButton.show()
#~ self.turnRadiansButton.move(0,self.turnRight90Button.y()+self.turnRight90Button.height())
#~ self.connect(self.turnRadiansButton, SIGNAL('clicked()'), self.turnRadiansClicked)
#~ self.turnRadiansButton.resize(250, 40)
#~
#~ self.angleLabel = QLabel(self)
#~ self.angleLabel.setText("Angle (radians):")
#~ self.angleLabel.resize(120, 70)
#~ self.angleLabel.move(self.turnRight90Button.x()+self.turnRight90Button.width(),self.turnRight90Button.y())
#~ self.angleLabel.show()
#~ self.angleSpinbox = QDoubleSpinBox(self)
#~ self.angleSpinbox.show()
#~ self.angleSpinbox.move(self.turnRadiansButton.x()+self.turnRadiansButton.width(),self.turnRadiansButton.y())
#~ self.angleSpinbox.resize(250, 40)
#~ self.angleSpinbox.setMaximum(3.14)
#~ self.angleSpinbox.setMinimum(-3.14)
#~ self.angleSpinbox.setSingleStep(0.1)
#~
#~
#~
#~
#~
#~
#~ self.rightSquareButton = QPushButton("Right Square", self)
#~ self.rightSquareButton.show()
#~ self.rightSquareButton.move(0,self.turnRadiansButton.y()+self.turnRadiansButton.height())
#~ self.connect(self.rightSquareButton, SIGNAL('clicked()'), self.rightSquareClicked)
#~ self.rightSquareButton.resize(250, 40)
#~
#~ self.leftSquareButton = QPushButton("Left Square", self)
#~ self.leftSquareButton.show()
#~ self.leftSquareButton.move(0,self.rightSquareButton.y()+self.rightSquareButton.height())
#~ self.connect(self.leftSquareButton, SIGNAL('clicked()'), self.leftSquareClicked)
#~ self.leftSquareButton.resize(250, 40)
#~
#~
#~ self.distanceLabel = QLabel(self)
#~ self.distanceLabel.setText("Distance (mm):")
#~ self.distanceLabel.resize(120, 70)
#~ self.distanceLabel.move(self.rightSquareButton.x()+self.rightSquareButton.width(),self.rightSquareButton.y())
#~ self.distanceLabel.show()
#~ self.distanceSpinbox = QDoubleSpinBox(self)
#~ self.distanceSpinbox.show()
#~ self.distanceSpinbox.move(self.leftSquareButton.x()+self.leftSquareButton.width(),self.leftSquareButton.y())
#~ self.distanceSpinbox.resize(250, 40)
#~ self.distanceSpinbox.setMaximum(5000.)
#~ self.distanceSpinbox.setMinimum(-5000.)
#~ self.distanceSpinbox.setSingleStep(10)
#~ self.advanceButton = QPushButton("Advance...", self)
#~ self.advanceButton.show()
#~ self.advanceButton.move(0,self.leftSquareButton.y()+self.leftSquareButton.height())
#~ self.connect(self.advanceButton, SIGNAL('clicked()'), self.moveForward)
#~ self.advanceButton.resize(250, 40)
self.connect(self.ui.advancebutton, SIGNAL('clicked()'),
self.moveForward)
self.connect(self.ui.turn90rightButton, SIGNAL('clicked()'),
self.right90Clicked)
self.connect(self.ui.turn90leftButton, SIGNAL('clicked()'),
self.left90Clicked)
self.connect(self.ui.turnRadiansButton, SIGNAL('clicked()'),
self.turnRadiansClicked)
self.connect(self.ui.rightSquareButton, SIGNAL('clicked()'),
self.rightSquareClicked)
self.connect(self.ui.leftSquareButton, SIGNAL('clicked()'),
self.leftSquareClicked)
self.job()
def job(self):
self.current_state = self.mods['RoboCompSmar'].TBaseState()
self.current_state = self.proxy.getBaseState()
self.ui.SmarPositionLabel.setText(
QString("X: " + str(self.current_state.x) + " Y: " +
str(self.current_state.z) + " Yaw: " +
str(self.current_state.alpha)))
#~ print "job"
#while(True):
#self.proxy.setPosition(goal1)
#time.sleep(6)
#self.proxy.setPosition(goal2)
#time.sleep(6)
#~ try:
#~ self.states = self.proxy.getAllMotorState()
#~ self.states = 0
#~ self.refused = 0
#~ state = self.states[str(self.combo.currentText())]
#~ self.readLabel.setText(QString.number(state.pos))
#~ except Ice.ConnectionRefusedException:
#~ if self.refused == 0:
#~ self.refused = 1
#~ print 'Connection refused'
#~ elif self.refused == 1:
#~ self.refused == 2
#self.clicked()
#~ TODO: CONTROLAR EXCEPCION DE GIRO SOBRE 0
def right90Clicked(self):
print "right90Clicked"
print "with radius of", self.ui.displaceVAlue.value(
), "m and Velocity:", self.ui.speedValue.value()
#~ worker->computeAPJE(radius, vel);
#get base state
self.old_state = self.mods['RoboCompSmar'].TBaseState()
self.new_state = self.mods['RoboCompSmar'].TBaseState()
self.old_state = self.proxy.getBaseState()
radius = math.fabs(self.ui.radiusValue.value())
base_vel = self.ui.speedValue.value()
self.proxy.setSpeedBase(0.0, radius)
#turn right
time.sleep(3)
self.proxy.setSpeedBase(base_vel, radius)
turning = True
reduced = False
#Wait until 90 turned
while (turning):
self.new_state = self.proxy.getBaseState()
#~ print "old"+str(self.old_state.alpha)
#~ print "new"+str(self.new_state.alpha)
distance = math.fabs(self.new_state.alpha - self.old_state.alpha)
if (distance >= (math.pi / 2)):
turning = False
print math.fabs(self.new_state.alpha - self.old_state.alpha)
elif (distance >= 1.2):
d = math.fabs(math.pi / 2. - distance)
print d
new_velocity = self.calculateVelocity(
base_vel, math.fabs(1.2 - math.pi / 2.), d)
print "new velocity ", new_velocity
if (new_velocity > 0.02):
self.proxy.setSpeedBase(new_velocity, radius)
time.sleep(0.1)
self.proxy.setSpeedBase(0.0, radius)
time.sleep(3)
self.proxy.setSpeedBase(0.0, float('inf'))
#~ TODO: CONTROLAR EXCEPCION DE GIRO SOBRE 0
def left90Clicked(self):
print "left90Clicked"
print "with radius of", self.ui.radiusValue.value(
), "m and Velocity:", self.ui.speedValue.value()
self.old_state = self.mods['RoboCompSmar'].TBaseState()
self.new_state = self.mods['RoboCompSmar'].TBaseState()
self.old_state = self.proxy.getBaseState()
radius = -math.fabs(self.ui.radiusValue.value())
base_vel = self.ui.speedValue.value()
if radius == 0:
base_vel = -math.fabs(base_vel)
self.proxy.setSpeedBase(0.0, -math.fabs(radius))
#turn right
time.sleep(3)
self.proxy.setSpeedBase(base_vel, -math.fabs(radius))
turning = True
reduced = False
#Wait until 90 turned
while (turning):
self.new_state = self.proxy.getBaseState()
#~ print "old"+str(self.old_state.alpha)
#~ print "new"+str(self.new_state.alpha)
distance = math.fabs(self.new_state.alpha - self.old_state.alpha)
if (distance >= (math.pi / 2)):
turning = False
print math.fabs(self.new_state.alpha - self.old_state.alpha)
elif (distance >= 1.2):
d = math.fabs(math.pi / 2. - distance)
print d
new_velocity = self.calculateVelocity(
base_vel, math.fabs(1.2 - math.pi / 2.), d)
if radius == 0:
new_velocity = -math.fabs(new_velocity)
print "new velocity ", new_velocity
if (new_velocity > 0.02):
self.proxy.setSpeedBase(new_velocity, -math.fabs(radius))
time.sleep(0.1)
self.proxy.setSpeedBase(0.0, -math.fabs(radius))
time.sleep(3)
self.proxy.setSpeedBase(0.0, float('inf'))
def calculateVelocity(self, base_vel, cut_pos, distance):
# print str(base_vel)+" - (("+str(base_vel)+"/"+str(cut_pos)+")*("+str(distance)+"-"+str(cut_pos)+"))"
# return base_vel - ((base_vel/cut_pos)*(distance-cut_pos))
# f = satura(K*disancia + C, Vm)
vel = (base_vel / cut_pos) * math.fabs(distance) + 0.02
if distance < 0: vel = -vel
if math.fabs(vel) > math.fabs(base_vel):
vel = base_vel
return vel
def waitRadians(self, radians):
pass
def waitAdvance(self, milimeters):
pass
def moveForward(self):
print "moveForward"
print "with avance of", self.ui.displaceVAlue.value(
), "mm and velocity", self.ui.speedValue.value()
self.old_state = self.mods['RoboCompSmar'].TBaseState()
self.new_state = self.mods['RoboCompSmar'].TBaseState()
self.old_state = self.proxy.getBaseState()
base_vel = self.ui.speedValue.value()
goal_dist = self.ui.displaceVAlue.value()
if goal_dist > 0:
self.proxy.setSpeedBase(base_vel, float('inf'))
else:
self.proxy.setSpeedBase(base_vel, float('-inf'))
advancing = True
while (advancing):
self.new_state = self.proxy.getBaseState()
xx = self.new_state.x - self.old_state.x
yy = self.new_state.z - self.old_state.z
actual_distance = math.sqrt(math.pow(xx, 2) + math.pow(yy, 2))
print "Actual distance: " + str(
actual_distance), self.new_state.alpha - self.old_state.alpha
if (actual_distance >= math.fabs(goal_dist)):
advancing = False
self.proxy.setSpeedBase(0.0, float('inf'))
else:
base_vel2 = base_vel
if math.fabs(actual_distance - math.fabs(goal_dist)) < 1000.:
base_vel2 = 0.3
elif math.fabs(actual_distance - math.fabs(goal_dist)) < 500.:
base_vel2 = 0.2
if math.fabs(actual_distance - math.fabs(goal_dist)) < 300.:
base_vel2 = 0.1
if goal_dist > 0:
self.proxy.setSpeedBase(base_vel2, float('inf'))
else:
self.proxy.setSpeedBase(base_vel2, float('-inf'))
time.sleep(0.1)
def rightSquareClicked(self):
print "rightSquareClicked"
print "with radius", self.ui.radiusValue.value(
), "and velocity", self.ui.speedValue.value()
self.moveForward()
self.right90Clicked()
time.sleep(1.8)
self.moveForward()
self.right90Clicked()
time.sleep(1.8)
self.moveForward()
self.right90Clicked()
time.sleep(1.8)
self.moveForward()
self.right90Clicked()
def leftSquareClicked(self):
print "leftSquareClicked"
print "with radius", self.ui.radiusValue.value(
), "and velocity", self.ui.speedValue.value()
self.moveForward()
self.left90Clicked()
time.sleep(1.8)
self.moveForward()
self.left90Clicked()
time.sleep(1.8)
self.moveForward()
self.left90Clicked()
time.sleep(1.8)
self.moveForward()
self.left90Clicked()
#TODO: COMPROBAR QUE LA VELOCIDAD NO ES 0
def turnRadiansClicked(self):
#~ print "NOT IMPLEMENTE YET"
print "turnRadiansClicked"
print self.ui.angDisplaceValue.value(
), "radians with radius", self.ui.radiusValue.value(
), "and velocity", self.ui.speedValue.value()
self.old_state = self.mods['RoboCompSmar'].TBaseState()
self.new_state = self.mods['RoboCompSmar'].TBaseState()
self.old_state = self.proxy.getBaseState()
radius = self.ui.radiusValue.value()
base_vel = self.ui.speedValue.value()
radians = self.ui.angDisplaceValue.value()
self.proxy.setSpeedBase(0.0, radius)
#turn right
time.sleep(3)
self.proxy.setSpeedBase(base_vel, radius)
turning = True
reduced = False
#Wait until 90 turned
while (turning):
self.new_state = self.proxy.getBaseState()
distance = math.fabs(self.new_state.alpha - self.old_state.alpha)
d = math.fabs(radians - distance)
print "Actual alpha: ", self.new_state.alpha - self.old_state.alpha
if (distance >= radians):
turning = False
print math.fabs(self.new_state.alpha - self.old_state.alpha)
elif (d < 0.3):
print d
new_velocity = self.calculateVelocity(base_vel, 0.3, d)
print "new velocity ", new_velocity
if (new_velocity > 0.02):
self.proxy.setSpeedBase(new_velocity, radius)
time.sleep(0.1)
self.proxy.setSpeedBase(0.0, radius)
time.sleep(3)
self.proxy.setSpeedBase(0.0, float('inf'))