SHARE
TWEET

Untitled

a guest Jul 21st, 2019 70 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. from gpiozero import DigitalInputDevice
  2.  
  3. from threading import Thread
  4. from time import sleep
  5.  
  6. from math import *
  7.  
  8. from adafruit_crickit import crickit
  9.  
  10. import sys
  11. import os
  12. def toDeg(x):
  13.     return x * 180/pi
  14.  
  15. def toRad(x):
  16.     return x / 180/pi
  17.  
  18. def toFixed(input, n):
  19.     input = str(input).split('.')
  20.     while len(input[1]) != n:
  21.         if len(input[1]) < n:
  22.             input[1] += "0"
  23.         else:
  24.             input[1] = input[1][:-1]
  25.     return input[0] + '.' + input[1]
  26.  
  27.  
  28. class TicksWatcher:
  29.     ticks = (0, 0)
  30.  
  31.     # GPIO pin BCM 23 & 24 Both pulled up
  32.     # gauche=14,15
  33.  
  34.     # droit=17,27
  35.  
  36.     phaseA = DigitalInputDevice(14, True)
  37.     phaseB = DigitalInputDevice(15, True)
  38.  
  39.     phaseC = DigitalInputDevice(17, True)
  40.     phaseD = DigitalInputDevice(27, True)
  41.  
  42.     counts = 0
  43.     countsP = 0
  44.     state = (0, 0)
  45.     oldState = (0, 0)
  46.     stateP = (0, 0)
  47.     oldStateP = (0, 0)
  48.  
  49.     thread = None
  50.  
  51.     def readTicks(self):
  52.         while True:
  53.             fetchedState = (self.phaseA.value, self.phaseB.value)
  54.             fetchedStateP = (self.phaseC.value, self.phaseD.value)
  55.             if fetchedState != self.state:
  56.                 self.state = fetchedState
  57.  
  58.                 if (self.state[0], self.oldState[1]) == (1, 0) or (self.state[0], self.oldState[1]) == (0, 1):
  59.                     self.counts += 1
  60.  
  61.                 elif (self.state[0], self.oldState[1]) == (1, 1) or (self.state[0], self.oldState[1]) == (0, 0):
  62.                     self.counts -= 1
  63.  
  64.                 self.oldState = self.state
  65.  
  66.             if fetchedStateP != self.stateP:
  67.                 self.stateP = fetchedStateP
  68.  
  69.                 if (self.stateP[0], self.oldStateP[1]) == (1, 0) or (self.stateP[0], self.oldStateP[1]) == (0, 1):
  70.                     self.countsP += 1
  71.  
  72.                 elif (self.stateP[0], self.oldStateP[1]) == (1, 1) or (self.stateP[0], self.oldStateP[1]) == (0, 0):
  73.                     self.countsP -= 1
  74.  
  75.                 self.oldStateP = self.stateP
  76.  
  77.     def start(self):
  78.         self.thread = Thread(target=self.readTicks)
  79.         self.thread.start()
  80.  
  81.     def getValues(self):
  82.         # gauche, droite
  83.         return (self.counts, self.countsP)
  84.  
  85.  
  86. class PositionWatcher:
  87.     oldTicks = (0, 0)
  88.     perimeter = 205
  89.     axialDistance = 233.5
  90.     theta = pi / 2
  91.     x = 0
  92.     y = 0
  93.     tickWatcher = TicksWatcher()
  94.  
  95.     def watchPosition(self):
  96.         while True:
  97.             newTicks = self.tickWatcher.getValues()
  98.             deltaTicks = (newTicks[0] - self.oldTicks[0],
  99.                           newTicks[1] - self.oldTicks[1])
  100.             self.oldTicks = newTicks
  101.             leftDistance = deltaTicks[0] / 2400 * self.perimeter
  102.             rightDistance = deltaTicks[1] / 2400 * self.perimeter
  103.             t1 = (leftDistance + rightDistance) / 2
  104.             alpha = (rightDistance - leftDistance) / self.axialDistance
  105.             self.theta = self.theta + alpha
  106.             self.x = self.x + t1 * cos(self.theta)
  107.             self.y = self.y + t1 * sin(self.theta)
  108.             # print((x - 95 * cos(theta), y - 95 * sin(theta)), theta * 180/pi)
  109.             sleep(0.01)
  110.  
  111.     def start(self):
  112.         self.tickWatcher.start()
  113.         self.thread = Thread(target=self.watchPosition)
  114.         self.thread.start()
  115.  
  116.     def getPos(self):
  117.         return (self.x, self.y)
  118.  
  119.     def getOrientation(self):
  120.         return (self.theta)
  121.  
  122.  
  123. positionWatcher = PositionWatcher()
  124. positionWatcher.start()
  125.  
  126. leftMotor = crickit.dc_motor_1
  127. rightMotor = crickit.dc_motor_2
  128.  
  129. seuil = 30
  130. pwmMin = 0.35
  131.  
  132. def pwm(targetTheta, theta, speed):
  133.     global pwmMin
  134.     return (1/speed - pwmMin) / (2*pi) * (targetTheta - theta) + 1
  135.  
  136. # def goTo(targetX, targetY):
  137. #     x = positionWatcher.getPos()[0]
  138. #     y = positionWatcher.getPos()[1]
  139. #     theta = positionWatcher.getOrientation()
  140. #     targetDistance = sqrt((targetX - x) ** 2 + (targetY - y) ** 2)
  141. #     targetTheta = atan2((targetY - y), (targetX - x))
  142. #     leftSpeed = rightSpeed = 0
  143. #     while targetDistance > seuil:
  144. #         print(targetDistance, targetTheta - theta, leftSpeed, rightSpeed)
  145. #         speed = ((1 + pwmMin) / 2)
  146. #         leftSpeed = speed * pwm(targetTheta, theta, speed)
  147. #         rightSpeed = speed * -pwm(targetTheta, theta, speed)      
  148. #         leftMotor.throttle = -leftSpeed
  149. #         rightMotor.throttle = -rightSpeed
  150.        
  151. #         x = positionWatcher.getPos()[0]
  152. #         y = positionWatcher.getPos()[1]
  153. #         theta = positionWatcher.getOrientation()
  154. #         targetDistance = sqrt((targetX - x) ** 2 + (targetY - y) ** 2)
  155. #         targetTheta = atan2((targetY - y), (targetX - x))
  156. #     leftMotor.throttle = 0
  157. #     rightMotor.throttle = 0
  158.  
  159. # def goToOrientation(targetOrientation):
  160. #     running = True
  161. #     seuilOrientation = pi/12
  162. #     cruiseSpeedOrientation = 0.5
  163. #     while running:
  164. #         theta = positionWatcher.getOrientation()
  165. #         print(theta / pi/180)
  166. #         deltaTheta = targetOrientation - theta
  167.        
  168. #         leftPwm = -((deltaTheta / abs(deltaTheta)) * cruiseSpeedOrientation)
  169. #         rightPwm = -((deltaTheta / abs(deltaTheta)) * cruiseSpeedOrientation)
  170. #         leftMotor.throttle = leftPwm
  171. #         rightMotor.throttle = rightPwm
  172.  
  173. #         if deltaTheta < seuilOrientation:
  174. #             running = False
  175.    
  176. #     leftMotor.throttle = 0
  177. #     rightMotor.throttle = 0
  178.    
  179. def goToOrientation(targetTheta):
  180.     seuilOrientation = pi/10
  181.     running = True
  182.     while running:
  183.         theta = positionWatcher.getOrientation()
  184.         deltaTheta = targetTheta - theta
  185.        
  186.         if abs(deltaTheta) > pi:
  187.             # autre sens
  188.             var = deltaTheta / abs(deltaTheta)
  189.             deltaTheta = (2*pi - abs(deltaTheta)) * -var    
  190.            
  191.         if abs(deltaTheta) > seuilOrientation:
  192.             leftPwm = 0.5 * deltaTheta/abs(deltaTheta) +(0.2/pi/(deltaTheta))
  193.             rightPwm = 0.5 * deltaTheta/abs(deltaTheta) +(0.2/pi/(deltaTheta))
  194.             leftMotor.throttle = leftPwm
  195.             rightMotor.throttle = rightPwm
  196.         else:
  197.             running = False
  198.            
  199.     leftMotor.throttle = 0
  200.     rightMotor.throttle = 0
  201.  
  202. def goTo(targetX, targetY, seuil = 30, endOrientation = None):
  203.     cruiseSpeed = 0.6
  204.    
  205.     # orienter le robot dans la bonne position    
  206.     x = positionWatcher.getPos()[0]
  207.     y = positionWatcher.getPos()[1]
  208.    
  209.     goToOrientation(atan2((targetY - y), (targetX - x)))
  210.     print('orientation fin')
  211.     running = True
  212.    
  213.     while running:        
  214.         x = positionWatcher.getPos()[0]
  215.         y = positionWatcher.getPos()[1]
  216.         theta = positionWatcher.getOrientation()
  217.         targetDistance = sqrt((targetX - x) ** 2 + (targetY - y) ** 2)
  218.         targetTheta = atan2((targetY - y), (targetX - x))
  219.         deltaTheta = targetTheta - theta
  220.         print(toDeg(deltaTheta))
  221.        
  222.         if abs(deltaTheta) > pi:
  223.             deltaTheta += 2*pi
  224.        
  225.         if abs(deltaTheta) < pi/2:
  226.             leftPwm =-(cruiseSpeed - (1-cruiseSpeed)*(deltaTheta/(pi / 2)))
  227.             rightPwm =(cruiseSpeed + (1-cruiseSpeed)*(deltaTheta/(pi / 2)))
  228.             leftMotor.throttle = leftPwm
  229.             rightMotor.throttle = rightPwm
  230.         else:
  231.             goToOrientation(atan2((targetY - y), (targetX - x)))
  232.                
  233.         if targetDistance < seuil:
  234.             running = False
  235.  
  236.     leftMotor.throttle = 0
  237.     rightMotor.throttle = 0
  238.    
  239.     if (endOrientation != None):
  240.         goToOrientation(toRad(endOrientation))
  241.    
  242. def main():    
  243.     goTo(-400, 0, 50)
  244.     goTo(-400, 500, 50)
  245.     goTo(400, 500, 50)
  246.     goTo(400, 1000, 50)
  247.     goTo(-200, 1000, 50)
  248.     goTo(-200, 1900, 50)
  249.     goTo(-200, 2000, 50)
  250.     goTo(30, 2000, 50, -90)
  251.     print("WOW")
  252.     goTo(-200, 2000, 50, -90)
  253.     goTo(-300, 1900, 50)
  254.     print("#-5")
  255.     goTo(-300, 1000, 50)
  256.     print("#-4")
  257.     goTo(400, 1000, 50)
  258.     print("#-3")
  259.     goTo(400, 500, 50)
  260.     print("#-2")
  261.     goTo(-400, 500, 50)
  262.     print("#-1")
  263.     goTo(-400, 0, 50)
  264.     print("#-0")
  265.     goTo(0, 0, 50, 90)
  266.     goToOrientation(pi/2)
  267.     print("LELEELLELELEL")
  268.    
  269.     #goTo(0, 400, 70)
  270.     # goTo(0, 700, 40)
  271.     # goTo(-500, 700, 40)
  272.     # goTo(600, 800, 50)
  273.     #print(positionWatcher.getOrientation() / pi/180)
  274.    
  275. try:
  276.     main()
  277. except KeyboardInterrupt:
  278.     print('Interrupted')    
  279.     leftMotor.throttle = 0
  280.     rightMotor.throttle = 0
  281.     goTo(0, 0, 70)
  282.     goToOrientation(pi/2)
  283.     try:
  284.         sys.exit(0)
  285.     except SystemExit:
  286.         os._exit(0)
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