SHARE
TWEET

Untitled

a guest Dec 14th, 2019 76 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. int emgVal1 = 0;
  2. int emgVal2 = 0; //Od 0 do 1023
  3. int tresh = 750;
  4. int motNapred[] = {11,12,13}; //palec pokazalec ostali
  5. int motNazad[] = {17,14,13};
  6. int lev[] = {0,0,0};
  7. int numLevels = 1;
  8. int vreme = 2000; //ima x leveli,najdi timing!
  9. int glavenDelay = 50;
  10. int niza[][3] = { {1,0,1}, {1,1,1}, {1,1,0}, {0,0,0} };
  11. bool vlezeni[3] = { false, false, false};
  12.  
  13. void setup() {
  14.   //Turi pinMode
  15.  
  16.   pinMode(A0, INPUT);
  17.   pinMode(A1, INPUT);
  18.   pinMode(motNapred[0], OUTPUT);
  19.   pinMode(motNapred[1], OUTPUT);
  20.   pinMode(motNapred[2], OUTPUT);
  21.   pinMode(motNazad[0], OUTPUT);
  22.   pinMode(motNazad[1], OUTPUT);
  23.   pinMode(motNazad[2], OUTPUT);
  24.  
  25.   Serial.begin(9600);
  26. }
  27.  
  28. int level(int val) {
  29.   //Od tresh do 1,2,3
  30.  
  31.   return ceil((val-tresh)/double(1023-tresh)*(numLevels));
  32. }
  33.  
  34. void mrdni(int smeni[3]) {
  35.   int razLeveli[3];
  36.  
  37.   for (int i = 0; i<3; i++) {
  38.      razLeveli[i] = smeni[i]-lev[i];
  39.  
  40.      if (vlezeni[i] == false) { digitalWrite(motNazad[i], HIGH); Serial.println("Teraj!"); }
  41.      if (vlezeni[i] == true) { digitalWrite(motNapred[i], HIGH); Serial.println("Vrakjaj!"); }
  42.   }
  43.  
  44.   int dileji[3] = {0,0,0};
  45.  
  46.   for (int i = 0; i<3; i++) dileji[i] = abs(razLeveli[i])*(vreme/numLevels);
  47.  
  48.   //Najdi raspored na delays
  49.  
  50.   int a = dileji[0], b = dileji[1], c = dileji[2];
  51.   int a2,b2,c2;
  52.  
  53.   if (a <= b & a <= c) {
  54.      if (b <= c) a2 = 0, b2 = 1, c2 = 2;
  55.      else a2 = 0, b2 = 2, c2 = 1;
  56.   } else if (b <= a && b <= c) {
  57.      if (a <= c) a2 = 1, b2 = 0, c2 = 2;
  58.      else a2 = 1, b2 = 2, c2 = 0;
  59.   } else if (c <= a && c <= b) {
  60.      if (a <= b) a2 = 2, b2 = 0, c2 = 1;
  61.      else a2 = 2, b2 = 1, c2 = 0;
  62.   }
  63.  
  64.   int rasp[3] = {a2,b2,c2};
  65.   int dileji2[3];
  66.  
  67.   dileji2[0] = dileji[rasp[0]];
  68.   dileji2[1] = dileji[rasp[1]]-dileji2[0];
  69.   dileji2[2] = dileji[rasp[2]]-dileji2[1];
  70.  
  71.   //Gotovo
  72.  
  73.   for (int i = 0; i<3; i++) {
  74.      lev[i] = smeni[i];
  75.  
  76.      if (razLeveli[i] <= 0) continue;
  77.  
  78.      delay(dileji2[i]);
  79.  
  80.      Serial.println(dileji2[i]);
  81.  
  82.     // if (vlezeni[i] == false) digitalWrite(motNazad[i], LOW);
  83.     // if (vlezeni[i] == true) digitalWrite(motNapred[i], LOW);
  84.  
  85.      vlezeni[i] = !vlezeni[i];
  86.   }
  87. }
  88.  
  89. void loop() {
  90.    emgVal1 = analogRead(A1);
  91.   // emgVal2 = analogRead(A2);
  92.    emgVal2 = 0;
  93.  
  94.    bool prv = emgVal1 >= tresh;
  95.    bool vtor = emgVal2 >= tresh;
  96.  
  97.    int lev1 = 0, lev2 = 0, levP = 0;
  98.  
  99.    //samo pokazalec (inverse), site 3, palec i pokazalec
  100.  
  101.    if (prv && !vtor) {    
  102.        lev1 = level(emgVal1);
  103.        
  104.      //  Serial.println("Palec, posledni 3!");
  105.     //   Serial.println(lev1, DEC);
  106.  
  107.        for (int i = 0; i<3; i++) if (niza[0][i]) niza[0][i] = lev1;
  108.    
  109.        mrdni(niza[0]);
  110.    } else if (!prv && vtor) {
  111.        lev2 = level(emgVal2);
  112.  
  113.      //  Serial.println("Palec, pokazalec, posledni 3!");
  114.      //  Serial.println(lev2, DEC);
  115.  
  116.        for (int i = 0; i<3; i++) if (niza[1][i]) niza[1][i] = lev2;
  117.        
  118.        mrdni(niza[1]);  
  119.    } else if (prv && vtor) {
  120.        levP = level( (emgVal1+emgVal2) / 2);
  121.  
  122.     //   Serial.println("Palec, pokazalec!");
  123.      //  Serial.println(levP, DEC);
  124.  
  125.        for (int i = 0; i<3; i++) if (niza[2][i]) niza[2][i] = levP;
  126.  
  127.        mrdni(niza[2]);
  128.    }
  129.  
  130.    delay(glavenDelay);
  131. }
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