API tools faq
paste
Advertisement
pabloducato

sw

pabloducato
Apr 6th, 2019
362
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 9.08 KB | None | 0 0
raw download clone embed print report
  1. /*************************************************************************/
  2. /*  Program testowy bez obiektu w oprogamowaniu SPSym                    */
  3. /*************************************************************************/
  4. #include "spsym.h"          // Pomocnicze funkcje i deklaracje
  5. #include <stdio.h>          // Stadnardowe I/O
  6. #include <math.h>
  7.  
  8. // --- Makrodefinicje dla obiektu X ---
  9. #define T  X8
  10.  
  11. #define Z1 Y1
  12. #define Z2 Y2
  13. #define Z3 Y3
  14. #define Z4 Y4
  15. #define Z5 Y5
  16. #define M  Y6
  17. #define G1 Y7
  18.  
  19. // --- Zmienne użytkownika ---
  20. int licz=0;                 // Licznik przerwań
  21. int stanAuta = 1, stanBieg = 1, lastBieg, stanPed = 1, stanTankowania = 1, stanZbiornika = 1, stanSygBieg = 1, stanPredkosci = 1;
  22. float tank = 0.50, v = 50;
  23. char refillingTank = 0, gear = 1, biegCount = 1, vCount = 0;
  24. char timAuto, timRefilTank, timTank, tank1, tank0, timBieg, timSpeed, speed0;
  25.  
  26. float acceleration (int gear, float v, int min, int max)
  27. {
  28.     float vWidth = max - min;
  29.     float wsp;
  30.     switch (gear)
  31.     {
  32.     case 1:
  33.         wsp = 1;
  34.         break;
  35.     case 2:
  36.         wsp = 1.5;
  37.         break;
  38.     case 3:
  39.         wsp = 2;
  40.         break;
  41.     case 4:
  42.         wsp = 0.8;
  43.         break;
  44.     case 5:
  45.         wsp = 0.4;
  46.         break;
  47.     }
  48.     return wsp * (-(pow((((v - min) / (vWidth/2)) - 1), 2)) + 2);
  49. }
  50.  
  51. void prolog(void)           // Inicjowanie programu (jednorazowo przy starcie)
  52. {
  53.     L1=L2=L3=L4=0;          // Zgaszenie LED-ów
  54.     L5=L6=L7=L8=0;          // Zgaszenie LED-ów
  55. }
  56.  
  57. void oblicz(void)           // Kod użytkownika wykonywany cyklicznie
  58. {
  59.     pK1=aK1; pK2=aK2; pK3=aK3; pK4=aK4;
  60.     pK5=aK5; pK6=aK6; pK7=aK7; pK8=aK8;
  61.  
  62.     tank1 = (int)(tank*20);
  63.     tank0 = 20 - tank1;
  64.  
  65.     if(gear)
  66.     {
  67.         if (v > 0) speed0 = (int)(v/10);
  68.         else speed0 = 0;
  69.         speed0 = 21 - speed0;
  70.     }
  71.     else
  72.     {
  73.         if (v < 0) speed0 = (int)((-v)/5);
  74.         else speed0 = 0;
  75.         speed0 = 7 - speed0;
  76.     }
  77.  
  78.     switch(stanAuta)
  79.     {
  80.     case -1:
  81.     {
  82.         refillingTank = 1;
  83.         if(!pK5 || !pK6) stanAuta = 0;
  84.     }
  85.         break;
  86.     case 0:
  87.     {
  88.         L1 = 0;
  89.         refillingTank = 0;
  90.         if(!pK5) stanAuta = 1;
  91.         else if(pK6) {stanAuta = -1; timRefilTank = 2;}
  92.     }
  93.         break;
  94.     case 1:
  95.     {
  96.         L1 = 0;
  97.         if(pK1 && tank > 0) stanAuta = 2;
  98.         else if(pK5 && !v) stanAuta = 0;
  99.     }
  100.         break;
  101.     case 2:
  102.     {
  103.         if(!pK1) {stanAuta = 3; timAuto = 5;}
  104.     }
  105.         break;
  106.     case 3:
  107.     {
  108.         L1 = 1;
  109.         if(!timAuto) {stanAuta = 4; timAuto = 5;}
  110.         else if(pK1) stanAuta = 5;
  111.     }
  112.         break;
  113.     case 4:
  114.     {
  115.         L1 = 0;
  116.         if(!timAuto) {stanAuta = 3; timAuto = 5;}
  117.         else if(pK1) stanAuta = 5;
  118.     }
  119.         break;
  120.     case 5:
  121.     {
  122.         L1 = 0;
  123.         if(!pK1 && gear == 1) stanAuta = 1;
  124.         else if(!pK1 && gear != 1) {stanAuta = 3; timAuto = 5;}
  125.     }
  126.         break;
  127.     }
  128.  
  129.     if(refillingTank)
  130.     {
  131.         switch(stanTankowania)
  132.         {
  133.         case 1:
  134.         {
  135.             L1 = 1;
  136.             if(!timRefilTank) {stanTankowania = 2; timRefilTank = 2;}
  137.         }
  138.             break;
  139.         case 2:
  140.         {
  141.             L1 = 0;
  142.             if(!timRefilTank) {stanTankowania = 1; timRefilTank = 2;}
  143.         }
  144.             break;
  145.  
  146.         }
  147.         tank+=0.005;
  148.         if(tank > 1) tank = 1;
  149.     }
  150.     else stanTankowania = 1;
  151.  
  152.     switch(stanZbiornika)
  153.     {
  154.     case 1:
  155.     {
  156.         L4 = 0;
  157.         timTank = tank1;
  158.         if(tank) stanZbiornika = 2;
  159.         else stanZbiornika = 4;
  160.     }
  161.         break;
  162.     case 2:
  163.     {
  164.         L4 = 1;
  165.         if(!timTank && tank < 1) {stanZbiornika = 3; timTank = tank0;}
  166.         else if(!tank) stanZbiornika = 4;
  167.     }
  168.         break;
  169.     case 3:
  170.     {
  171.         L4 = 0;
  172.         if(!timTank && tank > 0) {stanZbiornika = 2; timTank = tank1;}
  173.         else if(!tank) stanZbiornika = 5;
  174.     }
  175.         break;
  176.     case 4:
  177.     {
  178.         L4 = 0;
  179.         if(tank) {stanZbiornika = 3; timTank = tank0;}
  180.         else stanZbiornika = 5;
  181.     }
  182.     break;
  183.     case 5:
  184.     {
  185.         L4 = 1;
  186.         if(tank) {stanZbiornika = 3; timTank = tank0;}
  187.         else stanZbiornika = 4;
  188.     }
  189.     break;
  190.     }
  191.    
  192.     if(pK3 && stanAuta > 0)
  193.     {
  194.         if(gear && v) tank -= v * gear * 0.00000125;
  195.         else if(!gear && v < 0) tank -= (-v) * 0.00000125;
  196.        
  197.         if(tank <= 0)
  198.         {
  199.         tank = 0;
  200.         stanAuta = 1;
  201.         }
  202.     }
  203.  
  204.     if(gear)
  205.     {
  206.         switch(stanPredkosci)
  207.         {
  208.         case 1:
  209.         {
  210.             L3 = 0;
  211.             timSpeed = 21 - speed0;
  212.             stanPredkosci = 3;
  213.         }
  214.             break;
  215.         case 2:
  216.         {
  217.             L3 = 1;
  218.             if(!timSpeed) {stanPredkosci = 3; timSpeed = speed0;}
  219.         }
  220.             break;
  221.         case 3:
  222.         {
  223.             L3 = 0;
  224.             if(!timSpeed && v > 0) {stanPredkosci = 2; timSpeed = 1;}
  225.         }
  226.             break;
  227.         }
  228.     }
  229.     else
  230.     {
  231.         switch(stanPredkosci)
  232.         {
  233.         case 1:
  234.         {
  235.             L3 = 0;
  236.             timSpeed = 7 - speed0;
  237.             stanPredkosci = 3;
  238.         }
  239.             break;
  240.         case 2:
  241.         {
  242.             L3 = 1;
  243.             if(!timSpeed) {stanPredkosci = 3; timSpeed = speed0;}
  244.         }
  245.             break;
  246.         case 3:
  247.         {
  248.             L3 = 0;
  249.             if(!timSpeed && v < 0) {stanPredkosci = 2; timSpeed = 1;}
  250.         }
  251.             break;
  252.         }
  253.     }
  254.  
  255.     switch(gear)
  256.     {
  257.     case 0:
  258.     {
  259.         if(pK5 && !v) gear = 1;
  260.     }
  261.         break;
  262.     case 1:
  263.     {
  264.         if(pK2 && v >= 25) gear = 2;
  265.         else if(pK6 && !v && stanAuta > 1) gear = 0;
  266.     }
  267.         break;
  268.     case 2:
  269.     {
  270.         if(pK2 && v >= 60) gear = 3;
  271.         else if(v < 25) gear = 1;
  272.     }
  273.         break;
  274.     case 3:
  275.     {
  276.         if(pK2 && v >= 100) gear = 4;
  277.         else if(v < 60) gear = 2;
  278.     }
  279.         break;
  280.     case 4:
  281.     {
  282.         if(pK2 && v >= 150) gear = 5;
  283.         else if(v < 100) gear = 3;
  284.     }
  285.         break;
  286.     case 5:
  287.     {
  288.         if(v < 150) gear = 4;
  289.     }
  290.         break;
  291.     }
  292.  
  293.     if(pK4 && (stanAuta > 1 || v != 0))
  294.     {
  295.         if(gear)
  296.         {
  297.             if(v > 5) v -= v*0.02;
  298.             else v = 0;
  299.         }
  300.         else if(!gear)
  301.         {
  302.             if (v < 0) v += 1;
  303.         }
  304.     }
  305.     else if(pK3 && stanAuta > 1)
  306.     {
  307.         switch(gear)
  308.         {
  309.         case 0:
  310.         {
  311.             v -= acceleration(1, -v, 0, 30);
  312.             if (v < -30) v = -30;
  313.         }
  314.             break;
  315.         case 1:
  316.         {
  317.             v += acceleration(gear, v, 0, 30);
  318.             if (v > 30) v = 30;
  319.         }
  320.             break;
  321.         case 2:
  322.         {
  323.             v += acceleration(gear, v, 25, 75);
  324.             if (v > 75) v = 75;
  325.         }
  326.             break;
  327.         case 3:
  328.         {
  329.             v += acceleration(gear, v, 60, 100);
  330.             if (v > 100) v = 100;
  331.         }
  332.             break;
  333.         case 4:
  334.         {
  335.             v += acceleration(gear, v, 100, 160);
  336.             if (v > 160) v = 160;
  337.         }
  338.             break;
  339.         case 5:
  340.         {
  341.             v += acceleration(gear, v, 160, 200);
  342.             if (v > 200) v = 200;
  343.         }
  344.             break;
  345.         }
  346.     }
  347.     else if (v > 0)
  348.     {
  349.         v -= v*0.00125;
  350.         if(v < 0) v = 0;
  351.     }
  352.     else
  353.     {
  354.         v += 0.5;
  355.         if(v > 0) v = 0;
  356.     }
  357.  
  358.     switch(stanSygBieg)
  359.     {
  360.     case 1:
  361.     {
  362.         L2 = 0;
  363.         biegCount = gear;
  364.         if(biegCount)
  365.         {
  366.             timBieg = 2;
  367.             stanSygBieg = 2;
  368.         }
  369.         else L2 = 1;
  370.     }
  371.         break;
  372.     case 2:
  373.     {
  374.         L2 = 1;
  375.         if(!timBieg && biegCount > 1) {stanSygBieg = 3; timBieg = 2; biegCount--;}
  376.         else if(!timBieg && biegCount == 1) {stanSygBieg = 3; timBieg = 15; biegCount--;}
  377.     }
  378.         break;
  379.     case 3:
  380.     {
  381.         L2 = 0;
  382.         if(!timBieg && biegCount) {stanSygBieg = 2; timBieg = 2;}
  383.         else if(!timBieg && !biegCount) stanSygBieg = 1;
  384.     }
  385.         break;
  386.     }
  387.  
  388.     if(timAuto) --timAuto;
  389.     if(timRefilTank) --timRefilTank;
  390.     if(timTank) --timTank;
  391.     if(timBieg) --timBieg;
  392.     if(timSpeed) --timSpeed;
  393. }
  394.  
  395. void przerwanie(void)       // Obsługs przerwania od układu czasowo-licznikowego
  396. {}
  397.  
  398. void komunikacja(void)      // Obsługa komunikacji szeregowej
  399. {}
  400.  
  401. #ifdef Symulator_PC
  402. void wykres(void)           // Dane do tabeli i wykresu (dot. symulacji obiektu)
  403. {
  404.     aTab[0] = (int)(AI[0]/10.0);
  405.     aTab[1] = (int)(AI[1]/5.0);
  406.     aTab[2] = AI[7];
  407.     aTab[3] = (int)(licz/100.0);
  408.     aTab[4] = AO[0];
  409.     aTab[5] = AO[7];
  410.     aTab[6] = -120;
  411.     aTab[7] =  120;
  412.  
  413.     bTab[0] = (int)aK1;
  414.     bTab[1] = (int)aK2;
  415.     bTab[2] = (int)aK3;
  416.     bTab[3] = (int)aK4;
  417.     bTab[4] = (int)aK5;
  418.     bTab[5] = (int)aK6;
  419.     bTab[6] = (int)L1;
  420.     bTab[7] = (int)L2;
  421.     bTab[8] = (int)L3;
  422.     bTab[9] = (int)L4;
  423.     bTab[10] = (int)L5;
  424.     bTab[11] = (int)L6;
  425.     bTab[12] = 12;
  426.     bTab[13] = 13;
  427.     bTab[14] = 14;
  428.     bTab[15] = 15;
  429. }
  430. #endif
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
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
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!