Advertisement
cosenza987

Untitled

Aug 20th, 2022
89
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 6.42 KB | None | 0 0
  1. #!/usr/bin/env python3
  2.  
  3. from ev3dev2.motor import MediumMotor, LargeMotor, OUTPUT_A, OUTPUT_B, OUTPUT_C, MoveTank, SpeedDPS, SpeedRPS, SpeedRPM, MoveSteering
  4. from ev3dev2.led import Leds
  5. from ev3dev2.button import Button
  6. from ev3dev2.sensor.lego import TouchSensor, UltrasonicSensor, ColorSensor
  7. from ev3dev2.sensor import INPUT_1, INPUT_2, INPUT_3, INPUT_4
  8. from time import sleep
  9.  
  10. #definindo as variaveis
  11. btn = Button()
  12. us = UltrasonicSensor()
  13. usl = UltrasonicSensor(INPUT_2)
  14. usr = UltrasonicSensor(INPUT_3)
  15. cs = ColorSensor()
  16. csl = ColorSensor(INPUT_1)
  17. csr = ColorSensor(INPUT_4)
  18. mm = MediumMotor(OUTPUT_A)
  19. tank_pair = MoveTank(OUTPUT_B, OUTPUT_C)
  20. steer_pair = MoveSteering(OUTPUT_B, OUTPUT_C)
  21.  
  22. #as funcoes de procurar, atacar, retornar e saida
  23. def saida():
  24. #print("saida")
  25. #mm.on_for_degrees(speed = 70, degrees = -145, brake = True, block = True)
  26. if csl.value() > 26:
  27. tank_pair.on(left_speed = 0, right_speed = 40)
  28. while(csr.value() < 26):
  29. sleep(0.01)
  30. tank_pair.off(brake = True)
  31. tank_pair.on_for_degrees(left_speed = -50, right_speed = -50, degrees = 90, brake = True, block = True)
  32. tank_pair.on_for_rotations(left_speed = 65, right_speed = -40, rotations = 1, brake = False, block = True)
  33. tank_pair.on_for_degrees(left_speed = 70, right_speed = 70, degrees = 270, brake = False, block = True)
  34. return
  35. if csr.value() > 26:
  36. tank_pair.on(left_speed = 40, right_speed = 0)
  37. while(csl.value() < 26):
  38. sleep(0.01)
  39. tank_pair.off(brake = True)
  40. tank_pair.on_for_degrees(left_speed = -50, right_speed = -50, degrees = 90, brake = True, block = True)
  41. tank_pair.on_for_rotations(left_speed = -40, right_speed = 65, rotations = 1, brake = False, block = True)
  42. tank_pair.on_for_degrees(left_speed = 70, right_speed = 70, degrees = 270, brake = False, block = True)
  43. return
  44.  
  45. def voltar():
  46. #print("voltar")
  47. #mm.on_for_degrees(speed = 70, degrees = -145, brake = True, block = True)
  48. while cs.value() > 26:
  49. tank_pair.on(left_speed = -70, right_speed = -70)
  50. tank_pair.off()
  51. tank_pair.on_for_rotations(left_speed = -50, right_speed = -50, rotations = 1, brake = False, block = True)
  52. return
  53.  
  54. def atacar():
  55. #print("atacar")
  56. while True:
  57. tank_pair.on(left_speed = 70, right_speed = 70)
  58. #mm.on_for_degrees(speed = 70, degrees = 145, brake = True, block = False)
  59. if us.distance_centimeters > 15:
  60. tank_pair.off()
  61. return True
  62. if csl.value() > 26 and csr.value() > 26:
  63. tank_pair.off(brake = True)
  64. return False
  65. if csl.value() > 26 or csr.value() > 26:
  66. tank_pair.off(brake = True)
  67. break
  68. #if us.distance_centimeters < 10:
  69. #mm.on_for_degrees(speed = 70, degrees = 145, brake = True, block = True)
  70. #mm.on_for_degrees(speed = 70, degrees = -145, brake = True, block = False)
  71. saida()
  72.  
  73. def procurar():
  74. #print("procurar")
  75. #mm.on_for_degrees(speed = 70, degrees = -145, brake = True)
  76. sleep(0.01)
  77. if us.distance_centimeters < 65:
  78. tank_pair.on(left_speed = 70, right_speed = 70)
  79. sleep(0.3)
  80. if us.distance_centimeters < 15:
  81. return False
  82. else:
  83. return True
  84. while us.distance_centimeters > 65:
  85. #steer_pair.on(steering = 100, speed = 100)
  86. tank_pair.on(left_speed = -30, right_speed = 30)
  87. if us.distance_centimeters < 65:
  88. #steer_pair.off(brake = True)
  89. tank_pair.off(brake = True)
  90. return True
  91. if csl.value() > 26 or csr.value() > 26:
  92. saida()
  93. if btn.any():
  94. break
  95.  
  96.  
  97. def procuraproporcional(): #something that imo should work better, though you've got to remember that the sensors are quite shit
  98. mm.on_for_degrees(speed = 70, degrees = -145, brake = True)
  99. sleep(0.01)
  100. if usl.distance_centimeters < 65 and usr.distance_centimeters < 65:
  101. tank_pair.on(left_speed = 70, right_speed = 70)
  102. sleep(0.3)
  103. if usl.distance_centimeters < 15 and usr.distance_centimeters < 15:
  104. return False
  105. else:
  106. return True
  107. #pensando aqui, delta min pode ser por volta de 6 cm pra poder alinhar certo
  108. while usr.distance_centimeters > 65 and usl.distance_centimeters > 65:
  109. tank_pair.on(left_speed = -30, right_speed = 30)
  110. if btn.any():
  111. break
  112. if cs.value() > 26:
  113. saida()
  114. if usr.distance_centimeters < 75 and (abs(usr.distance_centimeters - usl.distance_centimeters) > 6):
  115. while abs(usr.distance_centimeters - usl.distance_centimeters) < 6:
  116. deltadistr = abs(usr.distance_centimeters - usl.distance_centimeters)
  117. errorr = deltadistr - 6
  118. if errorr > 30:
  119. errorr = 30
  120. if errorr < 15:
  121. errorr = 15
  122. if btn.any():
  123. break
  124. if cs.value() > 26:
  125. saida()
  126. tank_pair.on(left_speed = -errorr, right_speed = errorr)
  127. if usl.distance_centimeters < 75 and (abs(usr.distance_centimeters - usl.distance_centimeters) > 6):
  128. while abs(usr.distance_centimeters - usl.distance_centimeters) < 6:
  129. deltadistl = abs(usr.distance_centimeters - usl.distance_centimeters)
  130. errorl = deltadistl - 6
  131. if errorl > 30:
  132. errorl = 30
  133. if errorl < 15:
  134. errorl = 15
  135. if btn.any():
  136. break
  137. if cs.value() > 26:
  138. saida()
  139. tank_pair.on(left_speed = errorl, right_speed = -errorl)
  140.  
  141. #inicio
  142. while cs.value() < 26:
  143. tank_pair.on(left_speed = 50, right_speed = 50)
  144.  
  145. tank_pair.off(brake = True)
  146. tank_pair.on_for_degrees(left_speed = -50, right_speed = -50, degrees = 90, brake = True, block = True)
  147.  
  148. #girar
  149. tank_pair.on_for_rotations(left_speed = 50, right_speed = -50, rotations = 1, brake = False, block = True)
  150. #movimentacao inicial pra frente
  151. tank_pair.on_for_rotations(left_speed = 50, right_speed = 30, rotations = 1, brake = False, block = True)
  152. tank_pair.on_for_rotations(left_speed = 50, right_speed = 70, rotations = 1, brake = False, block = True)
  153.  
  154. while not btn.any():
  155. if not procurar():
  156. if not atacar():
  157. #voltar()
  158. sleep(0.001)
  159. sleep(.01)
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement