diff --git a/Main.py b/Main.py new file mode 100644 index 0000000..96ecf5a --- /dev/null +++ b/Main.py @@ -0,0 +1,73 @@ +##Vincen Andres Cabrera Yuco +##Sistema de control de un robot movil diferencial +#Ingenieria Mecatronica +import threading +import math as m +import numpy as np +#Clase de velovidad angular aplicada +class Velcanfapl: + + def __init__(self): + self.t=0 + self.theta=0 + self.v=0 + self.w=0 + + def vcart(self,v):#Variables de velocidad cartesiana + + mat1 = np.array([[m.cos(self.theta),m.sin(self.theta)], + [-m.sin(self.theta), m.cos(self.theta)], + [0, 1]]) + mat2=np.array([[v], + [self.w]]) + return np.dot(mat1, mat2) + + def vmov(self,mvcart):#Vector de movimiento en el marco local + mat1 = np.array([[m.cos(self.theta), -m.sin(self.theta), 0], + [-m.sin(self.theta), m.cos(self.theta), 0], + [0, 0, 1]]) + return np.dot(mat1, mvcart) + + def vang(self,mvmov,l):#Vector de velocidad angular aplicada + j1= np.array([[m.sin((m.pi/2)+(m.pi)), -m.cos((m.pi/2)+(m.pi)), -l*m.cos(m.pi)], + [m.sin((-(m.pi/2))), -m.cos(-(m.pi/2)), -l*m.cos(0)]]) + j2inv= np.array([[1/35, 0], + [0, 1/35]]) + mat1=np.dot(j2inv, j1) + return np.dot(mat1,mvmov) + + def restxt(self,mvang):#Funcion de registro de resultados + datos = np.column_stack((self.t, mvang[0], mvang[1])) + # Guardar el arreglo en un archivo txt + with open('resultado.txt', 'a', encoding='utf-8') as archivo: + for fila in datos: + # Convertir cada fila a una cadena de texto sin llaves + linea = ','.join(map(str, fila)) + archivo.write(linea + '\n') + + +velcanfapl= Velcanfapl() +with open('resultado.txt', 'a', encoding='utf-8') as archivo: + # Escribir el encabezado + archivo.write('t,𝜑Iz,𝜑De\n') +# Read the txt file +with open('datos.txt', 'r') as archivo: + archivo.readline() + for linea in archivo: + # Eliminar espacios en blanco al principio y al final de la línea + linea = linea.strip() + # Separar la línea por comas y convertir los elementos a enteros + numeros = [float(num) for num in linea.split(',')] + # Hacer algo con los números, por ejemplo, imprimirlos + velcanfapl.t=numeros[0] + velcanfapl.theta=numeros[1] + velcanfapl.v=numeros[2] + velcanfapl.w=numeros[3] + mvcart=velcanfapl.vcart(10) + mvmov=velcanfapl.vmov(mvcart) + mvang=velcanfapl.vang(mvmov,80) + velcanfapl.restxt(mvang) + + + + diff --git a/resultado.txt b/resultado.txt new file mode 100644 index 0000000..a865d56 --- /dev/null +++ b/resultado.txt @@ -0,0 +1,21 @@ +t,𝜑Iz,𝜑De +1.0,-0.05714285714285713,-0.5142857142857142 +2.0,0.857142857142857,-1.4285714285714284 +3.0,0.857142857142857,-1.4285714285714284 +4.0,1.0857142857142856,-1.657142857142857 +5.0,1.314285714285714,-1.8857142857142855 +6.0,0.6285714285714286,-1.2 +7.0,0.6285714285714284,-1.2 +8.0,0.3999999999999999,-0.9714285714285713 +9.0,0.1714285714285714,-0.7428571428571429 +10.0,-0.057142857142857016,-0.5142857142857142 +11.0,-0.057142857142857016,-0.5142857142857142 +12.0,-0.2857142857142857,-0.2857142857142857 +13.0,-0.28571428571428575,-0.2857142857142857 +14.0,-0.2857142857142858,-0.28571428571428575 +15.0,-0.2857142857142857,-0.2857142857142857 +16.0,0.1714285714285715,-0.7428571428571429 +17.0,1.542857142857143,-2.1142857142857143 +18.0,1.0857142857142854,-1.657142857142857 +19.0,0.857142857142857,-1.4285714285714284 +20.0,0.6285714285714286,-1.2