Codes - samirasancheze/EV3MeSy_Siegen_2018-2019 GitHub Wiki

Following the trajectory

While you are measuring the values of illuminace place the robot in white, in black and where you have 50% black and 50% white under the sensor, and write down these measurements. In the following example, we show some values obtained during the calibration:

• Black 5-11
• White 38-42
• 50/50 Black-White 25-29

As you can see in different positions black, white, and 50/50 gives different values, we take the average.

We show three different ways to solve the problem, in this section we are going to refer only to the color sensor:

1.Abrupt adjustment

• While the robot is near the border line 15 < il < 37 the robot drives forward, since it do not require any adjustment in its trajectory.
• If the robot is deviated toward the white area, then the sensor will be completely over this white area, in that condition il >= 38, so it has to come back and turns left.
• If the robot is deviated toward the black line, then the sensor will be completely over the black line, in that condition il <= 15, so it has to come back and turns right.
• If the sensor is looking at the infinity (e. from table to floor) it register il<3 and the motors stop.

 def motors_velocity(il,dist):
	motor_base=1.3
	if 15 < il < 37 :	# robot sigue derecho
		motor_v_L=motor_base
		motor_v_R=motor_base
	elif il >= 38:	# robot desvia a derecha
		print('blanco')
		motor_v_L=-0.8
		motor_v_R= 0.8		
	else:		#robot desvia a izquierda
		print('negro')
		motor_v_L=0.8
		motor_v_R=-0.8
	if il<3:
		motor_v_L=0
		motor_v_R=0		
	return motor_v_L, motor_v_R

2. Proportional 1

• If the sensor is looking to the infinity (e. from table to floor) it register il<3 and the motors stop.
• If the sensor is completely over a black area 3 < il<=17, the robot turns abruptly to right.
• If the sensor is more in black than in white area 17 < il <= midpos-d_negro, the robot turns gradually to right with a linear correction correccion=(midpos-il)/midpos.
• If the sensor is close to midpos midpos-d_negro < il < midpos+d_blanco the robot drives forward.
• If the sensor is more in white than in black area 38 > il >= midpos+d_blanco, the robot turns gradually to left with a linear correction correccion=(midpos-il)/midpos.
• If the sensor is completely over the white area il >= 38, turn abruptly to left.

def motors_velocity(il,dist):
	motor_v_L=0	# velocidad de motor
	motor_v_R=0	# velocidad de motor
	midpos=29  # (white-black)/2
	d_blanco=5
	d_negro=10
	motor_base=1.25
	motor_curva=0.8
	
	if il<=3:	# tortugueo (apaga los motores)
		motor_v_L= 0
		motor_v_R=0

	if 3 < il<=17:	# completamente sobre linea negra
		motor_v_L=  motor_curva 
		motor_v_R= -motor_curva

	if 17 < il <= midpos-d_negro:		# robot desvia un poco a izquierda
		print('black')
		correccion=(midpos-il)/midpos
		motor_v_L = motor_base
		motor_v_R= motor_base*(1-2*abs(correccion))
	
	if  midpos-d_negro < il < midpos+d_blanco:	# robot sigue derecho
		motor_v_L = motor_base
		motor_v_R = motor_base		

	if 38 > il >= midpos+d_blanco:	# robot desvia a derecha
		print('white')
		correccion = (midpos-il)/midpos
		if abs(correccion)>1:	# robot completamente en blanco
			correccion=1
		motor_v_L = motor_base*(1-2*abs(correccion))
		motor_v_R = motor_base		
	if il >= 38:	# curva fuerte hacia la izquierda (de blanco hacia negro)
		motor_v_L= -motor_curva
		motor_v_R=  motor_curva
	
	return motor_v_L, motor_v_R

3. Proportional 2

• If the sensor is looking to the infinity (e. table to floor) it register il<3 and the motors stop.
• If the sensor is completely over the black area 3 < il<=17, the robot turns abruptly to right.
• If the sensor is more in black than in white area 17 < il <= midpos, the robot turns gradually to right with a linear correction correccion=(midpos-il)/midpos.
• If the sensor is more in white than in black area 38 > il >= midpos+d_blanco, the robot turns gradually to left with a linear correction correccion=(midpos-il)/midpos.
• If the sensor is completely over the white area il >= 38, the robot turns abruptly to left.

def motors_velocity(il,dist):
	motor_v_L=0	# velocidad de motor
	motor_v_R=0	# velocidad de motor
	midpos=29  # (white-black)/2
	d_blanco=5
	d_negro=10
	motor_base=1.3
	motor_curva=0.8
	
	if il<=3:	# tortugueo (apaga los motores)
		motor_v_L= 0
		motor_v_R=0

	if 3 < il<=17:	# completamente sobre linea negra
		motor_v_L=  motor_curva 
		motor_v_R= -motor_curva

	if 17 < il <= midpos:		# robot desvia un poco a izquierda, a partir de la mitad
		print('black')
		correccion=(midpos-il)/midpos
		motor_v_L = motor_base
		motor_v_R= motor_base*(1-2*abs(correccion))	

	if midpos < il < 38:	# robot desvia a derecha, a partir de la mitad
		print('white')
		correccion = (midpos-il)/midpos
		#if abs(correccion)>1:	# robot completamente en blanco
		#	correccion=1
		motor_v_L = motor_base*(1-2*abs(correccion))
		motor_v_R = motor_base	
	if il>=38:
		motor_v_L= -motor_curva
		motor_v_R=  motor_curva	

	return motor_v_L, motor_v_R

---

It is important to mention that the path line had some imperfections that made the robot to read wrong information about the path and to deviate a little bit, sometimes the robot was exactly on the edge and the sensor registered values as if the robot was completely on a black zone.

The black zone have to be more opaque, not that bright.

---

Picking up and leaving the box

The algorithm is really simple:

• If the ultrasonic sensor register an obstacle closer than 11cm the robot stops

• If the robot doesn`t have a box

  • The robot moves down the arm
  • The robot drives forward
  • The robot moves up the arm
  • The robot turns left
  • Continue following the light

• If the robot already has a box

  • The robot moves down the arm
  • The robot drives backward
  • The robot moves up the arm
  • The robot turns left
  • Continue following the light

if distancia_m <= 0.11:# and distancia_m <= 0.125:	#the robot detect a box
if SensorData.caja==0:
	pub_motor_L.publish(Float64(0))		# The velocity of motor A
 	pub_motor_R.publish(Float64(0))	# The velocity of motor B
	rospy.sleep(.5)
	# move down the arm
	pub_motor_C.publish(Float64(-1))	# The velocity of motor B
	rospy.sleep(delay_palanca)
	pub_motor_C.publish(Float64(0))	# The velocity of motor B
	rospy.sleep(1)
	# Forward
	pub_motor_R.publish(Float64(1))	# The velocity of motor B
	pub_motor_L.publish(Float64(1))		# The velocity of motor A
	rospy.sleep(2)
	pub_motor_L.publish(Float64(0))		# The velocity of motor A
 	pub_motor_R.publish(Float64(0))	# The velocity of motor B
	rospy.sleep(1)
	# move up the arm
	pub_motor_C.publish(Float64(1))	# The velocity of motor C
	rospy.sleep(3.2)
	pub_motor_C.publish(Float64(0))	# The velocity of motor C
	rospy.sleep(1)
	# turn left
	pub_motor_L.publish(Float64(-1.4))	# The velocity of motor A
 	pub_motor_R.publish(Float64( 1.4))	# The velocity of motor B
	rospy.sleep(2)
	SensorData.caja=1
else:
	pub_motor_L.publish(Float64(0))		# The velocity of motor A
 	pub_motor_R.publish(Float64(0))		# The velocity of motor B
	rospy.sleep(.5)
	# move down the arm
	pub_motor_C.publish(Float64(-1))	# The velocity of motor B
	rospy.sleep(delay_palanca)
	pub_motor_C.publish(Float64(0))	# The velocity of motor B
	rospy.sleep(1)
	# Backward
	pub_motor_L.publish(Float64(-1))	# The velocity of motor A
 	pub_motor_R.publish(Float64(-1))	# The velocity of motor B
	rospy.sleep(1.7)
	pub_motor_L.publish(Float64(0))	# The velocity of motor A
 	pub_motor_R.publish(Float64(0))	# The velocity of motor B
	rospy.sleep(0.5)
	# move up the arm
	pub_motor_C.publish(Float64(1))	# The velocity of motor C
	rospy.sleep(3.2)
	pub_motor_C.publish(Float64(0))	# The velocity of motor C
	rospy.sleep(1)
	# turn left
	pub_motor_L.publish(Float64(-1.4))	# The velocity of motor A
 	pub_motor_R.publish(Float64( 1.4))	# The velocity of motor B
	rospy.sleep(2)
	SensorData.caja=0