Nantes Université

Skip to content
Extraits de code Groupes Projets
Valider d1bac49c rédigé par Mathis EMERIAU's avatar Mathis EMERIAU :cartwheel_tone1:
Parcourir les fichiers

homologation codes to modify (tournament version, it's trash).

parent bdc3724b
Aucune branche associée trouvée
Aucune étiquette associée trouvée
Aucune requête de fusion associée trouvée
Masquer les modifications d'espaces
En ligne Côte à côte
robot_a/code/strategy/homologation_left_main.py
+ 231
− 27
Voir le fichier @ d1bac49c
import HolonomeStepCalcul as h
import AccelStepperPY as pyccel
import Threads as t
#import Threads as t
import PositionLib as pl
import RPi.GPIO as GPIO
import math
import time
import serial
import numpy as np
import threading
# HOMOLOGATION CODE FOR THE LEFT SIDE (GREEN)
# CHANGERRRRRRRRRRRRRRRRRRRRRRRRRR
# HOMOLOGATION CODE FOR THE RIGHT SIDE (BLUE)
#######
# SET UP RASPBERRY AND MOTORS
......@@ -17,6 +20,7 @@ import numpy as np
GPIO.setmode(GPIO.BOARD) # Initialisation of the Raspberry on board numerotation
GPIO.setwarnings(False) # disable warnings
GPIO.cleanup()
pin_tirette = 3 # GPIO 2
GPIO.setup(pin_tirette, GPIO.IN)
......@@ -47,8 +51,6 @@ rx_xbee = 38 # GPIO 20
# Activation of the MOSFET for the motors and the arduino mega
mosfet_mega = 33 # GPIO 13
mosfet_motors = 35 # GPIO 19
GPIO.setup(mosfet_mega, GPIO.OUT)
GPIO.setup(mosfet_motors, GPIO.OUT)
#######
# DATA OF THE ROBOT
......@@ -59,21 +61,167 @@ dist_robot_center_wheel = 153 # dist_robot_center_wheel is the distance in mm be
radius_robot = 165
# Coordinates values in mm and angle in degree
current_position = pl.PositionRobot(pl.Point(225,285), 0)
# current_position = pl.PositionRobot(pl.Point(225,1815), 180)
current_position = pl.PositionRobot(pl.Point(225,285), 180)
acceleration_base = 100
max_speed_base = 300
acceleration_base = 100.0
max_speed_base = 300.0
#######
# CLASS
#######
class MovementThread(threading.Thread):
def __init__(self, changing_area, is_limited_area, ser):
threading.Thread.__init__(self)
self.waiting = False # to wait if there is an obstacle
self.stop_thread = False # to mandatory stop the robot at the end of the match
self.changing_area = changing_area # If we are changing of area because of the movement
self.limited_area = is_limited_area # Are we in a limited area before the movement
self.serial_port = ser # Serial port with the teensy
def run(self):
# We have to use global variables to modificate the motors
# even if the thread is killed at any moment
global moteur_1
global moteur_2
global moteur_3
global current_position
#self.communicationBeforeMovementToTeensy(self.serial_port)
# We stop the movement in 3 cases :
# a pause
# end of the movement
# the thread have to stop
nb_steps_done = 0
while((not self.stop_thread) and (moteur_1.distanceToGo() != 0
or moteur_2.distanceToGo() != 0
or moteur_3.distanceToGo() != 0)):
# We check if there is something to read
# else the read() function will block the execution
if self.serial_port.in_waiting > 0:
print("teensy detects something")
stoppingMovement = int.from_bytes(self.serial_port.read(), "little")
# if the teensy detects an object,
# it sends True and we stop the movement
# else if we don't detect anymore an object, we resume
if(stoppingMovement):
self.pause()
time.sleep(0.5)
self.serial_port.reset_input_buffer()
elif (not stoppingMovement):
self.resume()
print("resume")
# We have to check if the match isn't ended
self.verifyTimeForContinue()
if(not self.waiting):
moteur_1.run()
moteur_2.run()
moteur_3.run()
nb_steps_done = nb_steps_done + 1
# End of the movement (normal case)
print("wtf : ", nb_steps_done)
#self.communicationAfterMovementToTeensy(self.serial_port)
# True = Limited Area
# False = non Limited Area
def communicationBeforeMovementToTeensy(self, ser):
if(self.limited_area):
ser.write(bytes([True]))
elif(self.changing_area):
ser.write(bytes([True]))
else:
# we check it in case of
ser.write(bytes([False]))
# True = Limited Area
# False = non Limited Area
# The robot is arrived and we may have to modify range of detection
def communicationAfterMovementToTeensy(self, ser):
if(self.limited_area and self.changing_area):
ser.write(bytes([False]))
# Case already check before the movement, but double check
elif(self.limited_area and not self.changing_area):
ser.write(bytes([True]))
# Double check
elif(not self.limited_area and not self.changing_area):
ser.write(bytes([False]))
# Double check
elif(not self.limited_area and self.changing_area):
ser.write(bytes([True]))
def verifyTimeForContinue(self):
# the match is ended at 100 seconds, to have back up, we stop at 99 seconds
global start_time # date of the beginning of the match
if((time.time() - start_time) >= 99 ):
self.stop()
def pause(self):
self.waiting = True
print("pause")
def resume(self):
self.waiting = False
def stop(self):
self.stop_thread = True
print("stop")
class ActionsThread(threading.Thread):
def __init__(self, action, ser):
threading.Thread.__init__(self)
self.action = action # Action we want to realize
self.serial_port = ser # Serial port with the arduino mega
def run(self):
if(self.verifyTimeForContinue()):
action_to_do = self.action.encode('utf_8') # converts string in bytes
self.serial_port.write(action_to_do)
# We check if there is something to read
# else the read() function will block the execution
while self.serial_port.in_waiting > 0:
# We just want to have a response of the mega to continue (True normally)
response = int.from_bytes(self.serial_port.read(), "little")
print(response)
# the match is ended at 100 seconds, to have back up because of actions,
# we stop at 94 seconds to have back up
def verifyTimeForContinue(self):
global start_time # date of the beginning of the match
if((time.time() - start_time) >= 94 ):
return False
#######
# FUNCTIONS
#######
# Activate the mosfet to give the voltage to the motors and the mega
def activateMosfet(mosfet_motors, mosfet_mega):
GPIO.output(mosfet_motors, GPIO.HIGH)
GPIO.output(mosfet_mega, GPIO.HIGH)
# current_position is type of PositionRobot
# target_position is type of Point
# target_angle in degree
......@@ -97,8 +245,16 @@ def assignStepsToMotors(current_position, target_position, target_angle):
refMinSteps = h.nbStepsMin(steps)
tab = h.accelerationAndMaxSpeedEachMotor(steps, refMinSteps, acceleration_base, max_speed_base)
print("tab avant", tab)
print(steps)
for motor in range(3):
for i in range(2):
if( i == 0 and tab[motor][i] == 0.0):
tab[motor][i] = acceleration_base
if( i == 1 and tab[motor][i] == 0.0):
tab[motor][i] = max_speed_base
if motor == 0:
moteur_1.setAcceleration(tab[motor][i])
moteur_1.setMaxSpeed(tab[motor][i])
......@@ -109,6 +265,7 @@ def assignStepsToMotors(current_position, target_position, target_angle):
moteur_3.setAcceleration(tab[motor][i])
moteur_3.setMaxSpeed(tab[motor][i])
print("tab apres", tab)
moteur_1.move(steps[0])
moteur_2.move(steps[1])
moteur_3.move(steps[2])
......@@ -117,19 +274,21 @@ def assignStepsToMotors(current_position, target_position, target_angle):
# target_position is type of Point
# target_angle in degree
# serial_teensy is the instance of the virtual serial port for the teensy
# Function to realize a movement on the table (not actions) and
# return the target position to update robot's position
# Function to realize a movement on the table (not actions)
def doMovement(current_position, target_position, target_angle, serial_teensy):
global radius_robot
print("duyhbu")
print(" angle courant : ", current_position.angle)
# We don't move if the target position sends the robot into the wall
if(target_position.x - radius_robot > radius_robot or
target_position.y - radius_robot > radius_robot):
if(target_position.x - radius_robot > 0 or
target_position.y - radius_robot > 0):
print("oui")
assignStepsToMotors(current_position, target_position, target_angle)
changing_area = current_position.isChangingArea(target_position)
mov_thread = t.MovementThread(changing_area, current_position.limited_area, serial_teensy)
mov_thread = MovementThread(changing_area, current_position.limited_area, serial_teensy)
# We move to the target position and we wait to end the movement
mov_thread.start()
......@@ -150,6 +309,7 @@ def doMovement(current_position, target_position, target_angle, serial_teensy):
to push cakes.
'''
# Virtuals serial port to communicate with the teensy and the mega
# Open port with baud rate
serial_teensy = serial.Serial("/dev/ttyAMA0", 9600)
......@@ -158,29 +318,73 @@ time.sleep(0.1)
serial_mega = serial.Serial("/dev/ttyAMA0", 9600)
time.sleep(0.1)
print(GPIO.input(pin_tirette))
# We wait for the start
while(GPIO.input(pin_tirette)):
time.sleep(0.1)
print("lets go tirette")
start_time = time.time()
activateMosfet(mosfet_motors, mosfet_mega)
target_position = pl.Point(1550,285) # in mm
target_angle = 0 # in degree
'''
# GO IN THEN GO BACK
target_position = pl.Point(225,300) # in mm
target_angle = 180 # in degree
current_position = doMovement(current_position, target_position, target_angle, serial_teensy)
target_position = pl.Point(225,1815) # in mm
target_angle = 180 # in degree
current_position = doMovement(current_position, target_position, target_angle, serial_teensy)
'''
target_position = pl.Point(470,285) # in mm
target_angle = 180 # in degree
current_position = doMovement(current_position, target_position, target_angle, serial_teensy)
target_position = pl.Point(1550,1065) # in mm
target_angle = 0 # in degree
target_position = pl.Point(470,1100) # in mm
target_angle = 180 # in degree
current_position = doMovement(current_position, target_position, target_angle, serial_teensy)
target_position = pl.Point(1775,1065) # in mm
target_angle = 0 # in degree
target_position = pl.Point(225,1100) # in mm
target_angle = 180 # in degree
current_position = doMovement(current_position, target_position, target_angle, serial_teensy)
target_position = pl.Point(1775,285) # in mm
target_angle = 0 # in degree
target_position = pl.Point(225,300) # in mm
target_angle = 180 # in degree
current_position = doMovement(current_position, target_position, target_angle, serial_teensy)
target_position = pl.Point(225,850) # in mm
target_angle = 180 # in degree
current_position = doMovement(current_position, target_position, target_angle, serial_teensy)
target_position = pl.Point(725,850) # in mm
target_angle = 180 # in degree
current_position = doMovement(current_position, target_position, target_angle, serial_teensy)
target_position = pl.Point(725,1775) # in mm
target_angle = 180 # in degree
current_position = doMovement(current_position, target_position, target_angle, serial_teensy)
target_position = pl.Point(725,1125) # in mm
target_angle = 180 # in degree
current_position = doMovement(current_position, target_position, target_angle, serial_teensy)
target_position = pl.Point(225,1775) # in mm
target_angle = 180 # in degree
current_position = doMovement(current_position, target_position, target_angle, serial_teensy)
robot_a/code/strategy/homologation_right_main.py
+ 23
− 31
Voir le fichier @ d1bac49c
......@@ -60,7 +60,8 @@ dist_robot_center_wheel = 153 # dist_robot_center_wheel is the distance in mm be
radius_robot = 165
# Coordinates values in mm and angle in degree
current_position = pl.PositionRobot(pl.Point(225,285), 0)
# current_position = pl.PositionRobot(pl.Point(225,1815), 180)
current_position = pl.PositionRobot(pl.Point(225,285), 180)
acceleration_base = 100.0
max_speed_base = 300.0
......@@ -327,71 +328,62 @@ print("lets go tirette")
start_time = time.time()
target_position = pl.Point(450,285) # in mm
target_angle = 0 # in degree
'''
# GO IN THEN GO BACK
target_position = pl.Point(225,300) # in mm
target_angle = 180 # in degree
current_position = doMovement(current_position, target_position, target_angle, serial_teensy)
target_position = pl.Point(450,285) # in mm
target_position = pl.Point(225,1815) # in mm
target_angle = 180 # in degree
current_position = doMovement(current_position, target_position, target_angle, serial_teensy)
'''
target_position = pl.Point(450,1100) # in mm
target_position = pl.Point(470,285) # in mm
target_angle = 180 # in degree
current_position = doMovement(current_position, target_position, target_angle, serial_teensy)
target_position = pl.Point(225,1100) # in mm
target_position = pl.Point(470,1100) # in mm
target_angle = 180 # in degree
current_position = doMovement(current_position, target_position, target_angle, serial_teensy)
target_position = pl.Point(225,1100) # in mm
target_angle = 360 # in degree
target_angle = 180 # in degree
current_position = doMovement(current_position, target_position, target_angle, serial_teensy)
target_position = pl.Point(225,285) # in mm
target_angle = 0 # in degree
target_position = pl.Point(225,300) # in mm
target_angle = 180 # in degree
current_position = doMovement(current_position, target_position, target_angle, serial_teensy)
target_position = pl.Point(225,285) # in mm
target_position = pl.Point(225,850) # in mm
target_angle = 180 # in degree
current_position = doMovement(current_position, target_position, target_angle, serial_teensy)
target_position = pl.Point(225,2050) # in mm
target_position = pl.Point(725,850) # in mm
target_angle = 180 # in degree
current_position = doMovement(current_position, target_position, target_angle, serial_teensy)
'''
target_position = pl.Point(225,2000) # in mm
target_angle = 0 # in degree
target_position = pl.Point(725,1775) # in mm
target_angle = 180 # in degree
current_position = doMovement(current_position, target_position, target_angle, serial_teensy)
target_position = pl.Point(225,400) # in mm
target_angle = 0 # in degree
target_position = pl.Point(725,1125) # in mm
target_angle = 180 # in degree
current_position = doMovement(current_position, target_position, target_angle, serial_teensy)
'''
'''
for i in range(20):
target_position = pl.Point(225,285) # in mm
target_angle = 360 # in degree
current_position = doMovement(current_position, target_position, target_angle, serial_teensy)
target_position = pl.Point(225,1775) # in mm
target_angle = 180 # in degree
current_position = doMovement(current_position, target_position, target_angle, serial_teensy)
'''
'''
moteur_1.setAcceleration(100.0)
moteur_1.setMaxSpeed(300.0)
moteur_1.move(200)
while(moteur_1.distanceToGo() != 0):
moteur_1.run()
'''
0% ou .
You are about to add 0 people to the discussion. Proceed with caution.
Terminez d'abord l'édition de ce message.
Veuillez vous inscrire ou vous pour commenter