Autopilot with ada
In this projet, I have created an autopilot in ada language.
The program ask for the origin and destination airport coordenates, altitude and flight speed.
After that, it runs like an autopilot, when it writes "derecha" or "izquierda" (right or left) it would be sending this information to the system that control the aleiron servomotor. In the same way, when it writes "arriba" or " "abajo", it is managing the elevators.
The full code is shown below.
WITH Ada.Text_IO; USE Ada.Text_IO;
WITH NT_Console; USE NT_Console;
WITH Ada.Integer_Text_Io; USE Ada.Integer_Text_Io;
with ada.Float_Text_IO;use ada.Float_Text_IO;
WITH Ada.Calendar;USE Ada.Calendar;
with gps; USE gps;
WITH System; USE System;
WITH Ada.Synchronous_Task_Control;USE Ada.Synchronous_Task_Control;
PROCEDURE gps1 IS
Lat1,Long1,Lat2,Long2,Vel1,Alt1,Latact,Longact,Altact,Velact:Float;
rumboter,rumboact:float;
Hora,Minuto,Segundo,D,Idaa:Integer;
Arab:Integer range 0..2;
Cadrad:Integer RANGE 1..8;
--si cadrad=1 dir este,cadrad=2 dir noreste, si cadrad=3 dir norte,
--si cadrad=4 dir noroeste, si cadrad=5 dir oeste, si cadrad=6 dir suroeste
--si cadrad=7 dir sur, si cadrad 8 dir sureste.
TASK TYPE Mhora;
TASK TYPE Mpos IS
ENTRY Getpos(La1,Lon1:OUT Float);
end mpos;
task type malt;
TASK TYPE Mvel;
TASK TYPE Tarab;
task type mrumbo;
TYPE A IS ACCESS Mhora;
TYPE B IS ACCESS Mpos;
TYPE C IS ACCESS Malt;
TYPE E IS ACCESS Mvel;
TYPE F IS ACCESS Tarab;
TYPE g IS ACCESS Mrumbo;
Mh1:A;
Mp1:B;
Mal1:C;
Mve1:E;
Marab1:F;
Mrum1:G;
TASK Accesopantalla IS
ENTRY agoto (x,y:in integer);
ENTRY Aput(D: IN Integer;idaa:in integer);
END Accesopantalla;
-- proceso mostar hora
procedure Muestra_Hora is
BEGIN
Set_Foreground (Yellow);
Put_Line(Integer'Image(Hora)&
":"&Integer'Image(Minuto)&":"&Integer'Image(Segundo));
END Muestra_Hora;
-- fin proceso mostar hora
-- proceso mostar posición
procedure Muestra_pos is
BEGIN
Set_Foreground (Yellow);
Put (Latact, 2, 2, 0);
put(' ');
Put (Longact, 2, 2, 0);
END Muestra_pos;
-- fin proceso mostar posición
-- proceso mostar altitud
procedure Muestra_alt is
BEGIN
Set_Foreground (Yellow);
Put (altact, 9, 1, 0);
END Muestra_alt;
-- fin proceso mostar altitud
-- proceso mostar velocidad
procedure Muestra_vel is
BEGIN
Set_Foreground (Yellow);
Put (velact, 9, 1, 0);
END Muestra_vel;
-- fin proceso mostar velocidad
-- proceso mostar arriba-abajo
procedure Muestra_Arab is
BEGIN
Put(" ");
Goto_Xy(40,8);
IF Arab=0 then
Set_Foreground (red);
Put ("ABAJO");
elsif arab=1 then
Set_Foreground (red);
Put ("ARRIBA");
else
Set_Foreground (green);
Put ("ALTURA CORRECTA");
end if;
END Muestra_Arab;
-- fin proceso mostar arriba-ajo
-- proceso mostar rumbo
procedure Muestra_rumbo is
BEGIN
Set_Foreground (green);
Put (Rumboter, 2, 2, 0);
Put(' ');
Set_Foreground (YELLOW);
Put (rumboact, 2, 2, 0);
Put(" ");
Goto_Xy(52,6);
Set_Foreground (RED);
CASE Cadrad IS
WHEN 1 =>
IF Rumboact-Rumboter>0.001 THEN Put (" DERECHA");
ELSIF Rumboact-Rumboter<-0.001 THEN Put (" IZQUIERDA");
ELSE
Set_Foreground (green);
Put(" RUMBO CORRECTO");
END IF;
WHEN 2 =>
IF Rumboact-Rumboter>0.001 THEN Put (" DERECHA");
ELSIF Rumboact-Rumboter<-0.001 THEN Put (" IZQUIERDA");
ELSE
Set_Foreground (green);
Put(" RUMBO CORRECTO");
end if;
WHEN 3 =>
IF Rumboact-Rumboter>0.001 and Rumboact<90.0 THEN Put (" IZQUIERDA");
ELSIF Rumboact-Rumboter>0.001 AND Rumboact>90.0 THEN Put (" DERECHA");
ELSE
Set_Foreground (green);
Put(" RUMBO CORRECTO");
end if;
WHEN 4 =>
IF Rumboact-Rumboter>0.001 THEN Put (" DERECHA");
ELSIF Rumboact-Rumboter<-0.001 THEN Put (" IZQUIERDA");
ELSE
Set_Foreground (green);
Put(" RUMBO CORRECTO");
END IF;
WHEN 5 =>
IF Rumboact-Rumboter>0.001 THEN Put (" DERECHA");
ELSIF Rumboact-Rumboter<-0.001 THEN Put (" IZQUIERDA");
ELSE
Set_Foreground (green);
Put(" RUMBO CORRECTO");
END IF;
WHEN 6 =>
IF Rumboact-Rumboter>0.001 THEN Put (" DERECHA");
ELSIF Rumboact-Rumboter<-0.001 THEN Put (" IZQUIERDA");
ELSE
Set_Foreground (green);
Put(" RUMBO CORRECTO");
END IF;
WHEN 7 =>
IF Rumboact-Rumboter<-0.001 AND rumboact>90.0 THEN Put (" IZQUIERDA");
ELSIF Rumboact-Rumboter<-0.001 AND Rumboact<90.0 THEN Put (" DERECHA");
ELSE
Set_Foreground (green);
Put(" RUMBO CORRECTO");
END IF;
WHEN 8 =>
IF Rumboact-Rumboter>0.001 THEN Put (" DERECHA");
ELSIF Rumboact-Rumboter<-0.001 THEN Put (" IZQUIERDA");
ELSE
Set_Foreground (green);
Put(" RUMBO CORRECTO");
END IF;
end case;
END Muestra_rumbo;
-- fin proceso mostar rumbo
--tarea periódica mostar hora
TASK BODY mhora IS
X1:Integer:=25;
Y1:Integer:=12;
instante:time;
Period: CONSTANT Duration :=1.0;
Next_Time:Time:=Clock;
BEGIN
LOOP
DELAY UNTIL Next_Time;
Instante:=Clock;
Hora:= Integer(Seconds(Instante))/3600;
Minuto:= (Integer(Seconds(Instante))-Hora*3600)/60;
Segundo:= (Integer(Seconds(Instante))-Hora*3600-Minuto*60);
Accesopantalla.Agoto(X1,Y1);
Accesopantalla.Aput(1,0);
Next_Time:=Next_Time+Period;
END LOOP;
END mhora;
--fin tarea periódica mostar hora
--tarea periódica mostar posición
TASK BODY Mpos IS
X2:Integer:=25;
Y2:Integer:=6;
Period: CONSTANT Duration :=0.5;
Next_Time:Time:=Clock;
BEGIN
LOOP
DELAY UNTIL Next_Time;
ACCEPT getpos(la1,lon1:out float) DO
Pos(Latact,Longact);
end;
Accesopantalla.Agoto(X2,Y2);
Accesopantalla.Aput(2,0);
Next_Time:=Next_Time+Period;
END LOOP;
END mpos;
--fin tarea periódica mostar posición
--tarea periódica mostar altitud
TASK BODY Malt IS
X3:Integer:=25;
Y3:Integer:=8;
Period: CONSTANT Duration :=0.5;
Next_Time:Time:=Clock;
BEGIN
LOOP
DELAY UNTIL Next_Time;
alt(altact);
Accesopantalla.Agoto(X3,Y3);
Accesopantalla.Aput(3,0);
Next_Time:=Next_Time+Period;
END LOOP;
END malt;
--fin tarea periódica mostar altitud
--tarea periódica mostar velocidad
TASK BODY Mvel IS
X4:Integer:=25;
Y4:Integer:=10;
Posant,Posact: Posicion;
Period: CONSTANT Duration :=2.0;
Next_Time:Time:=Clock;
La1,Lon1: Float;
BEGIN
Posant.Lg:=0.0;
Posant.Lt:=0.0;
LOOP
DELAY UNTIL Next_Time;
Mp1.All.getpos(La1,Lon1);
Posact.Lg:=lon1;
Posact.Lt:=La1;
velact:=(distancia(posact,posant,Altact))/0.5;
Accesopantalla.Agoto(X4,Y4);
Accesopantalla.Aput(4,0);
Posant.Lg:=Longact;
Posant.Lt:=Latact;
Next_Time:=Next_Time+Period;
END LOOP;
END mvel;
--fin tarea periódica mostar altitud
--tarea periódica arriba-abajo
TASK BODY tarab IS
X5:Integer:=40;
Y5:Integer:=8;
Period: CONSTANT Duration :=0.3;
Next_Time:Time:=Clock;
BEGIN
LOOP
DELAY UNTIL Next_Time;
IF Altact>Alt1 THEN
Arab:=0;
ELSIF Altact<Alt1 THEN Arab:=1;
ELSE Arab:=2;
END IF;
Accesopantalla.Agoto(X5,Y5);
Accesopantalla.Aput(5,0);
Next_Time:=Next_Time+Period;
END LOOP;
END tarab;
--fin tarea periódica arriba-abajo
--tarea periódica rumbo
TASK BODY mrumbo IS
X6:Integer:=40;
Y6:Integer:=6;
Posini,Posdest:Posicion;
Period: CONSTANT Duration :=0.3;
Next_Time:Time:=Clock;
Posact:Posicion;
BEGIN
Posini.Lg:=long1;
Posini.Lt:=Lat1;
Posdest.Lg:=Long2;
Posdest.Lt:=Lat2;
IF Lat2>Lat1 AND Long2=Long1 THEN Cadrad:=1;
elsif Lat2>Lat1 AND Long2>Long1 THEN Cadrad:=2;
ELSIF Lat2=Lat1 AND Long2>Long1 THEN Cadrad:=3;
ELSIF Lat2<Lat1 AND Long2>Long1 THEN Cadrad:=4;
ELSIF Lat2<Lat1 AND Long2=Long1 THEN Cadrad:=5;
ELSIF Lat2<Lat1 AND Long2<Long1 THEN Cadrad:=6;
ELSIF Lat2=Lat1 AND Long2<Long1 THEN Cadrad:=7;
ELSE Cadrad:=8;
END IF;
LOOP
DELAY UNTIL Next_Time;
Rumboter:=Rumbo(Posdest,Posini);
pos(posact.lt,posact.lg);
rumboact:=rumbo(posdest,posact);
Accesopantalla.Agoto(X6,Y6);
Accesopantalla.Aput(6,0);
Next_Time:=Next_Time+Period;
END LOOP;
END mrumbo;
--fin tarea rumbo
-- tarea semaforo
TASK BODY Accesopantalla IS
BEGIN
LOOP
ACCEPT Agoto(x,y: in integer) do
Goto_XY(X,Y);
end;
ACCEPT Aput(D:IN Integer;Idaa:IN Integer) DO
Set_Foreground (white);
IF D=1 THEN
Muestra_Hora;
ELSIF D=2 THEN
Muestra_Pos;
ELSIF D=3 THEN
Muestra_Alt;
ELSIF D=4 THEN
Muestra_Vel;
Elsif D=5 THEN
Muestra_Arab;
ELSIF D=6 THEN
muestra_rumbo;
ELSE
Put_Line("Error en parámetros");
end if;
end;
END LOOP;
end accesopantalla;
BEGIN
Set_Foreground (White);
Goto_Xy(10,5);
Put_Line("Introduce longitud del aeropuerto de origen");
Set_Foreground (yellow);
goto_xy(55,5);
Get(Long1);
Set_Foreground (White);
Goto_Xy(10,6);
Put_Line("Introduce latitud del aeropuerto de origen");
Set_Foreground (yellow);
goto_xy(55,6);
Get(Lat1);
Set_Foreground (White);
--
Goto_Xy(10,7);
Put_Line("Introduce longitud del aeropuerto de destino");
Set_Foreground (yellow);
goto_xy(55,7);
Get(Long2);
Set_Foreground (White);
Goto_Xy(10,8);
Put_Line("Introduce latitud del aeropuerto de destino");
Set_Foreground (Yellow);
goto_xy(55,8);
Get(Lat2);
Set_Foreground (White);
goto_xy(10,9);
Put_Line("Introduce velocidad de crucero en Km/h");
Set_Foreground (yellow);
goto_xy(55,9);
Get(vel1);
Set_Foreground (White);
goto_xy(10,10);
Put_Line("Introduce Altitud de crucero en m");
Set_Foreground (yellow);
goto_xy(55,10);
Get(alt1);
DELAY(1.5);
-- limpiar pantalla
goto_xy(0,0);
FOR I IN 0..2000 LOOP
Put(' ');
END LOOP;
--
Inicia(Lat1,Long1,Lat2,Long2,alt1);
Set_Velocidad(Vel1);
-- mostramos marco invariante
Set_Foreground (White);
goto_xy(5,6);
Put_Line("Posicion actual");
goto_xy(5,8);
Put_Line("Altitud actual");
goto_xy(5,10);
Put_Line("Velocidad Actual");
Goto_Xy(5,12);
Put_Line("Hora actual");
-- lanzamos las tareas periódicas
Mh1:=NEW mhora;
Mp1:=NEW Mpos;
Mal1:=NEW Malt;
Mve1:=NEW Mvel;
Marab1:=NEW Tarab;
mrum1:= new mrumbo;
NULL;
end gps1;