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:
- 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
- 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 correctioncorreccion=(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 correctioncorreccion=(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
- 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 correctioncorreccion=(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 correctioncorreccion=(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