daily pastebin goal
88%
SHARE
TWEET

Untitled

a guest Jan 24th, 2019 117 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #!/usr/bin/env python3
  2. from ev3dev2.motor import LargeMotor, OUTPUT_D,OUTPUT_A,  OUTPUT_C, SpeedPercent, MoveTank
  3. from ev3dev2.sensor import INPUT_2,INPUT_1,INPUT_3,INPUT_4
  4. from ev3dev2.sensor.lego import TouchSensor,ColorSensor
  5. from ev3dev2.led import Leds
  6. from ev3dev2.sound import Sound
  7. import time
  8.  
  9.  
  10. ts = TouchSensor(INPUT_3)
  11. l_right = ColorSensor(INPUT_2)
  12. l_left = ColorSensor(INPUT_4)
  13.  
  14. sound = Sound()
  15.  
  16. #tank_drive = MoveTank(OUTPUT_A, OUTPUT_D)
  17.  
  18. drive_left = LargeMotor(OUTPUT_A)
  19. drive_right = LargeMotor(OUTPUT_D)
  20.  
  21.  
  22.  
  23. leds = Leds()
  24.  
  25. time.sleep(0.1)
  26. leds.set_color("LEFT", "GREEN")
  27. leds.set_color("RIGHT", "RED")
  28. time.sleep(0.1)
  29. leds.set_color("LEFT", "RED")
  30. leds.set_color("RIGHT", "GREEN")
  31. time.sleep(0.1)
  32. leds.set_color("LEFT", "GREEN")
  33. leds.set_color("RIGHT", "GREEN")
  34. time.sleep(0.1)
  35.  
  36. tolerance = 4 #aprox difference between black and white
  37. is_running = False
  38. Kp = 10
  39. Ki = 0
  40. Kd = 0
  41.  
  42. limit = 300
  43. speed = 200
  44.  
  45. dt = 200
  46.  
  47.  
  48. integral = 0
  49. previous_error = 0
  50.  
  51. sound.speak('Hello world')
  52. try:
  53.     while True:
  54.         if(ts.is_pressed):
  55.             is_running = not is_running
  56.             time.sleep(0.3)
  57.         if(not is_running):
  58.             drive_right.stop()
  59.             drive_left.stop()
  60.             continue
  61.  
  62.         error = l_left.reflected_light_intensity - l_right.reflected_light_intensity
  63.         integral += (error * dt)
  64.         derivative = (error - previous_error) / dt
  65.  
  66.         u=(Kp*error) + (Ki * integral) + (Kd * derivative)
  67.         print(u)
  68.         previous_error = error
  69.         # u>0 -> turn right
  70.         # u<0 -> turn left
  71.        
  72.        
  73.         #run
  74.         if(u>=0):#bright from the left side
  75.                 #turn right
  76.             drive_left.run_forever(speed_sp=min(speed+u,limit))
  77.             drive_right.run_forever(speed_sp=max(speed-u,-limit))
  78.         else:#bright from the right side
  79.                   #turn left
  80.             drive_left.run_forever(speed_sp=max(speed+u,-limit))
  81.             drive_right.run_forever(speed_sp=min(speed-u,limit))
  82.            
  83.  
  84.  
  85.  
  86.  
  87.         # if((l_left.reflected_light_intensity - tolerance) > l_right.reflected_light_intensity):#needs to turn left
  88.         #   drive_left.run_forever(speed_sp=100)
  89.         #   drive_right.run_forever(speed_sp=-100)
  90.         # elif((l_left.reflected_light_intensity + tolerance) < l_right.reflected_light_intensity):#needs to turn right
  91.         #   drive_left.run_forever(speed_sp=-100)
  92.         #   drive_right.run_forever(speed_sp=100)
  93.         # else:
  94.         #   drive_left.run_forever(speed_sp=200)
  95.         #   drive_right.run_forever(speed_sp=200)
  96.  
  97.  
  98.  
  99.  
  100. except KeyboardInterrupt:
  101.     drive_right.stop()
  102.     drive_left.stop()
  103. print('hey')
  104.     # if ts.
  105.     #is_pressed:
  106.     #   sound.speak('intensity of light is ' + str(l_left.reflected_light_intensity()))
  107.  
  108.     # if ts.is_pressed:
  109.     #   tank_drive.on_for_rotations(SpeedPercent(-25), SpeedPercent(25), 10)
  110.     #   tank_drive.run_forever()
  111.     # else:
  112.     #   tank_drive.stop()
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top