------------------------------------------------------------------------------- -- Controla Posicionador 2D (completo) -- -- Asignatura: ITR Curso: 2005/2006 ------------------------------------------------------------------------------- with MaRTE_OS; with IO_Interface; use IO_Interface; with MaRTE_Hardware_Interrupts; use MaRTE_Hardware_Interrupts; with Basic_Integer_Types; use Basic_Integer_Types; with Text_IO; use Text_IO; with Ada.Float_Text_IO; use Ada.Float_Text_IO; with Ada.Calendar; use Ada.Calendar; with Ada.Exceptions, System; procedure Controla_Posicionador_2D is -- Parámetros de la plataforma Mx_X : constant Float := 20.0; -- metros Mx_Y : constant Float := 10.0; -- metros Mx_Vel_X : constant Float := 2.0; -- m/s Mx_Vel_Y : constant Float := 1.0; -- m/s Pos_Ini_X : constant Float := Mx_X / 2.0; -- metros Pos_Ini_Y : constant Float := Mx_Y / 2.0; -- metros -- Direcciones de los registros de E/S del convertidor BASE_AX5411 : constant IO_Port := 16#300#; Reg_ADL : constant IO_Port := BASE_AX5411 + 0; Reg_ADH : constant IO_Port := BASE_AX5411 + 1; Reg_CGA : constant IO_Port := BASE_AX5411 + 1; Reg_MUC : constant IO_Port := BASE_AX5411 + 2; Reg_START : constant IO_Port := BASE_AX5411 + 0; Reg_CLI : constant IO_Port := BASE_AX5411 + 8; Reg_STATUS : constant IO_Port := BASE_AX5411 + 8; Reg_CNTR : constant IO_Port := BASE_AX5411 + 9; Reg_DAL0 : constant IO_Port := BASE_AX5411 + 4; Reg_DAH0 : constant IO_Port := BASE_AX5411 + 5; Reg_DAL1 : constant IO_Port := BASE_AX5411 + 6; Reg_DAH1 : constant IO_Port := BASE_AX5411 + 7; Reg_DOUT : constant IO_Port := BASE_AX5411 + 3; -- Direciones de los registros del puerto paralelo PP_BASE : constant IO_Port := 16#378#; -- Puerto paralelo 1 PP_DATA_REG : constant IO_Port := PP_BASE + 0; -- Registro de datos PP_STATUS_REG : constant IO_Port := PP_BASE + 1; -- Registro de estado PP_CONTROL_REG : constant IO_Port := PP_BASE + 2; -- Registro de control -- Modo de operación type Modos_Operacion is (Modo_Teclado, Modo_Remoto); Modo_Operacion : Modos_Operacion := Modo_Teclado; pragma Volatile (Modo_Operacion); -- es "volatile" porque se comparte entre -- más de una tarea --------------------------------------------------------------------------- -- Modo teclado --------------------------------------------------------- --------------------------------------------------------------------------- -- Objeto protegido para la consigna en modo teclado protected Consigna_Teclado is pragma Priority (5); procedure Lee (Consigna_X, Consigna_Y : out Float); procedure Escribe (Consigna_X, Consigna_Y : in Float); private X : Float := Pos_Ini_X; Y : Float := Pos_Ini_Y; end Consigna_Teclado; protected body Consigna_Teclado is procedure Lee (Consigna_X, Consigna_Y : out Float) is begin Consigna_X := X; Consigna_Y := Y; end Lee; procedure Escribe (Consigna_X, Consigna_Y : in Float) is begin X := Consigna_X; Y := Consigna_Y; end Escribe; end Consigna_Teclado; -- Tarea de lectura del teclado task Lee_Teclado is pragma Priority (4); end Lee_Teclado; task body Lee_Teclado is Key : Character; Pulsada : Boolean; Coord_X, Coord_Y : Float; begin loop Get_Immediate (Key, Pulsada); if Pulsada then case Key is when 't' | 'T' => Modo_Operacion := Modo_Teclado; when 'r' | 'R' => Modo_Operacion := Modo_Remoto; when others => null; end case; end if; case Modo_Operacion is when Modo_Teclado => Put ("Coordenada X"); Get (Coord_X); Skip_Line; Put ("Coordenada Y"); Get (Coord_Y); Skip_Line; Consigna_Teclado.Escribe (Coord_X, Coord_Y); when Modo_Remoto => null; end case; end loop; exception when E : others => Put ("Exception in Lee_Teclado: "); Put (Ada.Exceptions.Exception_Name (E) & " "); Put (Ada.Exceptions.Exception_Message (E)); end Lee_Teclado; ---------------------------------------------------------------------------- -- Modo control remoto (Puerto paralelo) --------------------------------- ---------------------------------------------------------------------------- -- Consigna en modo control remoto Consigna_Remota_X, Consigna_Remota_Y : Unsigned_8; pragma Atomic (Consigna_Remota_X); pragma Volatile (Consigna_Remota_X); pragma Atomic (Consigna_Remota_Y); pragma Volatile (Consigna_Remota_Y); -- Manejador de interrupción del puerto paralelo Contador_Puerto_Paralelo : Integer := 0; Consigna_Remota_X_Tmp : Unsigned_8; function Manejador_Puerto_Paralelo (Area : in System.Address; Intr : in Hardware_Interrupt) return Handler_Return_Code is begin if Contador_Puerto_Paralelo = 0 then -- Lee la consigna X y la guarda en una variable temporal Consigna_Remota_X_Tmp := Inb (PP_DATA_REG); else -- Lee la consigna Y y actualiza los valores de -- 'Consigna_Remota_Y' y 'Consigna_Remota_Y' Consigna_Remota_Y := Inb (PP_DATA_REG); Consigna_Remota_X := Consigna_Remota_X_Tmp; end if; Contador_Puerto_Paralelo := (Contador_Puerto_Paralelo + 1) mod 2; return POSIX_INTR_HANDLED_NOTIFY; end Manejador_Puerto_Paralelo; ---------------------------------------------------------------------------- -- Manejador de la interrupción de la tarjeta AX5411 --------------------- -- (lectura de la posición actual) --------------------- ---------------------------------------------------------------------------- Posicion_AD_X, Posicion_AD_Y : Unsigned_16; pragma Atomic (Posicion_AD_X); pragma Volatile (Posicion_AD_X); pragma Atomic (Posicion_AD_Y); pragma Volatile (Posicion_AD_Y); Contador_AX5411 : Integer := 0; function Manejador_AD_AX5411 (Area : in System.Address; Intr : in Hardware_Interrupt) return Handler_Return_Code is begin if Contador_AX5411 = 0 then -- Lee canal 0 (posición en el eje X) Posicion_AD_X := Unsigned_16 (Inb (REG_ADH)) * 2**4 + Unsigned_16 (Inb (REG_ADL)) / 2**4; else -- Lee canal 1 (posición en el eje Y) Posicion_AD_Y := Unsigned_16 (Inb (REG_ADH)) * 2**4 + Unsigned_16 (Inb (REG_ADL)) / 2**4; end if; Contador_AX5411 := (Contador_AX5411 + 1) mod 2; -- Comienza la siguiente conversión Outb (Reg_CLI, 0); Outb (Reg_START, 0); return POSIX_INTR_HANDLED_NOTIFY; end Manejador_AD_AX5411; ---------------------------------------------------------------------------- -- Tarea de control de posición ------------------------------------------ ---------------------------------------------------------------------------- task Controla_Posicion is pragma Priority (5); end Controla_Posicion; task body Controla_Posicion is Proxima_Activacion : Ada.Calendar.Time := Ada.Calendar.Clock; Periodo : constant Duration := 0.1; Kp_X : constant Float := 400.0; Kp_Y : constant Float := 300.0; Posicion_X, Posicion_Y : Float; Consigna_X, Consigna_Y : Float; Vel : Float; Consigna_Motor_X, Consigna_Motor_Y : Unsigned_16; begin ------------ ---- Configura puerto paralelo ---- -- Instala manejador de interrupción if Associate (PARALLEL1_INTERRUPT, Manejador_Puerto_Paralelo'Unrestricted_Access, System.Null_Address, 0) /= 0 then Put_Line ("Error: Associate"); end if; -- Configura como puerto de entrada y habilita las -- interrupciones del puerto paralelo Outb (PP_CONTROL_REG, 2#00_1_1_0000#); -- Habilita la interrupción en el PIC if Unlock (PARALLEL1_INTERRUPT) /= 0 then Put_Line ("Error: Unlock"); end if; ------------ ---- Configura AX5411 ---- -- Instala manejador de interrupción if Associate (PARALLEL2_INTERRUPT, Manejador_AD_AX5411'Unrestricted_Access, System.Null_Address, 0) /= 0 then Put_Line ("Error: Associate"); end if; Outb (Reg_CGA, 16#00#); -- Ganancia 1 Outb (Reg_MUC, 16#10#); -- Lee canales 0 y 1 Outb (Reg_CNTR, 2#1_101_0_0_00#); -- Int. habilitada -- tipo 5 (paralelo 2) -- Habilita la interrupción en el PIC if Unlock (PARALLEL2_INTERRUPT) /= 0 then Put_Line ("Error: Unlock"); end if; -- Provoca primera conversión Outb (Reg_CLI, 0); Outb (Reg_START, 0); ------------ ---- Lazo de control ---- loop Proxima_Activacion := Proxima_Activacion + Periodo; delay until Proxima_Activacion; case Modo_Operacion is when Modo_Teclado => Consigna_Teclado.Lee (Consigna_X, Consigna_Y); when Modo_Remoto => -- Obtiene las consignas X e Y enviadas por el -- controlador remoto. Se hace con interrupciones -- deshabilitadas para crear una sección crítica con -- el manejador de interrupción del puerto paralelo, y -- de esta forma leer siempre una pareja (X,Y) -- coherente. if Lock (PARALLEL1_INTERRUPT) /= 0 then Put_Line ("Error: Unlock"); end if; Consigna_X := Float (Consigna_Remota_X) * Mx_X / 16#FF.0#; Consigna_Y := Float (Consigna_Remota_Y) * Mx_Y / 16#FF.0#; if Unlock (PARALLEL1_INTERRUPT) /= 0 then Put_Line ("Error: Unlock"); end if; end case; -- Lee posición actual Posicion_X := Float (Posicion_AD_X) * Mx_X / 16#FFF.0#; Posicion_Y := Float (Posicion_AD_Y) * Mx_Y / 16#FFF.0#; -- Algoritmo de control de posición para la X Vel := (Consigna_X - Posicion_X) * Kp_X; if Vel >= Mx_Vel_X then Consigna_Motor_X := 16#FFF#; -- Valor máximo positivo elsif Vel <= -Mx_Vel_X then Consigna_Motor_X := 16#0#; -- Valor máximo negativo else -- "Vel" dentro de límites Consigna_Motor_X := Unsigned_16 (Abs (Vel) * (16#FFF.0#/2.0) / Mx_Vel_X); if Vel > 0.0 then Consigna_Motor_X := Consigna_Motor_X + 16#FFF# / 2; else Consigna_Motor_X := 16#FFF# / 2 - Consigna_Motor_X; end if; end if; -- Algoritmo de control de posición para la Y Vel := (Consigna_Y - Posicion_Y) * Kp_Y; if Vel >= Mx_Vel_Y then Consigna_Motor_Y := 16#FFF#; -- Valor máximo positivo elsif Vel <= -Mx_Vel_Y then Consigna_Motor_Y := 16#0#; -- Valor máximo negativo else -- "Vel" dentro de límites Consigna_Motor_Y := Unsigned_16 (Abs (Vel) * (16#FFF.0#/2.0) / Mx_Vel_Y); if Vel > 0.0 then Consigna_Motor_Y := Consigna_Motor_Y + 16#FFF# / 2; else Consigna_Motor_Y := 16#FFF# / 2 - Consigna_Motor_Y; end if; end if; -- Pone consignas en los motores Outb (Reg_DAL0, Unsigned_8 ((Consigna_Motor_X * 2**4) and 16#FF#)); Outb (Reg_DAH0, Unsigned_8 ((Consigna_Motor_X / 2**4) and 16#FF#)); Outb (Reg_DAL1, Unsigned_8 ((Consigna_Motor_Y * 2**4) and 16#FF#)); Outb (Reg_DAH1, Unsigned_8 ((Consigna_Motor_Y / 2**4) and 16#FF#)); end loop; exception when E : others => Put ("Exception in Controla_Posicion: "); Put (Ada.Exceptions.Exception_Name (E) & " "); Put (Ada.Exceptions.Exception_Message (E)); end Controla_Posicion; begin null; end Controla_Posicionador_2D;