daily pastebin goal
94%
SHARE
TWEET

Untitled

a guest May 28th, 2018 59 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include "hFramework.h"
  2. #include <Lego_Touch.h>
  3. #include <stddef.h>
  4. #include <stdio.h>
  5. #include <hGPIO.h>
  6.  
  7. using namespace hSensors;
  8. using namespace hFramework;
  9.  
  10. #define napedTyl hMot1
  11. #define napedSrodek hMot2
  12. #define podnosnikPrzod hMot3
  13. #define podnosnikTyl hMot4
  14. #define skrecanie hMot5
  15. #define napedTyl2 hMot6
  16.  
  17. void hMain()
  18. {
  19.     podnosnikPrzod.setEncoderPolarity(Polarity::Reversed);  //changing encoder polarity (Polarity::Normal is default)
  20.     podnosnikPrzod.setMotorPolarity(Polarity::Reversed);  //changing motor polarity
  21.  
  22.     podnosnikTyl.setEncoderPolarity(Polarity::Reversed);  //changing encoder polarity (Polarity::Normal is default)
  23.     podnosnikTyl.setMotorPolarity(Polarity::Reversed);  //changing motor polarity
  24.  
  25.     hExt1.pin1.setIn_pu();      // guzik przod
  26.     hExt1.pin2.setIn_pu();      // guzik srodek
  27.     hExt1.pin3.setIn_pu();      // guzik tyl
  28.  
  29.     //SINLNIKI
  30.     //1 - NAPED tyl
  31.     //2 - NAPED srodek
  32.     //3 - PONDOSNIK PRZOD
  33.     //4 - PODNOSNIK TYL
  34.     //5 - SKRECANIE
  35.     //6 - naped tyl
  36.  
  37.     int liftPower = 600;
  38.     int ridePower = 225;
  39.     int pressPower = 55;
  40.  
  41.     //PIERWSSZY SCHOD
  42.             //jazda do przodu poki guzik 1przod nie jest wcisniety
  43.             while(hExt1.pin1.read() )
  44.             {
  45.                 napedSrodek.setPower(-ridePower);
  46.                 napedTyl.setPower(-ridePower);
  47.                 napedTyl2.setPower(-ridePower);
  48.                 Serial.printf("szukam schodow \n");
  49.             }
  50.             napedTyl.setPower(0);
  51.             napedTyl2.setPower(0);
  52.             napedSrodek.setPower(0);
  53.  
  54.             //podnoszenie 1 podwozia po dojezdzie przodu do schodka
  55.             while(!hExt1.pin1.read())
  56.             {
  57.                 podnosnikPrzod.setPower(liftPower);
  58.                 napedSrodek.setPower(-pressPower);
  59.                 napedTyl.setPower(-pressPower);
  60.                 napedTyl2.setPower(-pressPower);
  61.                 Serial.printf("podnosze przod \n");
  62.             }
  63.             podnosnikPrzod.setPower(0);
  64.             podnosnikPrzod.rotRel(1500, liftPower, true, INFINITE);
  65.  
  66.     while(true)
  67.     {
  68.         //jazda do przodu az do dotkniecia srodka ze schodkiem
  69.         while(hExt1.pin2.read())
  70.         {
  71.             napedSrodek.setPower(-ridePower);
  72.             napedTyl.setPower(-ridePower);
  73.             napedTyl2.setPower(-ridePower);
  74.             Serial.printf("jade do przodu  1\n");
  75.         }
  76.         napedTyl.setPower(0);
  77.         napedTyl2.setPower(0);
  78.         napedSrodek.setPower(0);
  79.  
  80.         //podnoszenie srodka po dojezdzie srodka
  81.         while(!hExt1.pin2.read())
  82.         {
  83.             podnosnikPrzod.setPower(-liftPower);
  84.             podnosnikTyl.setPower(liftPower);
  85.             napedTyl.setPower(-pressPower);
  86.             napedTyl2.setPower(-pressPower);
  87.             Serial.printf("podnosze srodek \n");
  88.         }
  89.         podnosnikPrzod.setPower(0);
  90.         podnosnikTyl.setPower(0);
  91.         sys.delay_ms(2000);
  92.  
  93.  /*       for(int i=0; i<=5; i++)
  94.         {
  95.             podnosnikPrzod.rotRel(-210, 1000, true, INFINITE);
  96.             podnosnikTyl.rotRel(210, 1000, true, INFINITE);
  97.         } */
  98.  
  99.         //jazda do przodu az do dojazdu przodu po raz drugi
  100.         while(hExt1.pin1.read())
  101.         {
  102.             napedSrodek.setPower(-ridePower);
  103.             napedTyl.setPower(-ridePower);
  104.             napedTyl2.setPower(-ridePower);
  105.             Serial.printf("jade do przodu  2\n");
  106.         }
  107.         hMot1.setPower(0);
  108.         hMot2.setPower(0);
  109.         hMot6.setPower(0);
  110.  
  111.         //podnoszenie 1 podwozia po deojezdzie przodu do schodka
  112.         while(!hExt1.pin1.read())
  113.         {
  114.             podnosnikPrzod.setPower(liftPower);
  115.             napedSrodek.setPower(-pressPower);
  116.             napedTyl.setPower(-pressPower);
  117.             napedTyl2.setPower(-pressPower);
  118.             Serial.printf("podnosze przod \n");
  119.         }
  120.         podnosnikPrzod.setPower(0);
  121.         podnosnikPrzod.rotRel(1500, liftPower, true, INFINITE);
  122.  
  123.         //jazda do przodu az do dotkniecia tylu
  124.         while(hExt1.pin3.read())
  125.         {
  126.             hMot1.setPower(-ridePower);
  127.             hMot2.setPower(-ridePower);
  128.             hMot6.setPower(-ridePower);
  129.             Serial.printf("jade do przodu 3\n");
  130.         }
  131.         hMot1.setPower(0);
  132.         hMot2.setPower(0);
  133.         hMot6.setPower(0);
  134.  
  135.         //podnoszenie tylu po dojezdzie tylu do schodka
  136.         while(!hExt1.pin3.read())
  137.         {
  138.             podnosnikTyl.setPower(-liftPower);
  139.             napedSrodek.setPower(-pressPower);
  140.             napedTyl.setPower(-pressPower);
  141.             napedTyl2.setPower(-pressPower);
  142.             Serial.printf("podnosze tyl \n");
  143.         }
  144.         podnosnikTyl.setPower(0);
  145.         podnosnikTyl.rotRel(-1500, liftPower, true, INFINITE);
  146.     }
  147. }
RAW Paste Data
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
 
Top