Advertisement
lolcraftdolly

MasterCode

Nov 29th, 2023
1,302
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #define analogInput 12 // entrada analogica do microfone
  2. #define digitalInput 13 // entrada digital do microfone
  3. #define tower1 14
  4. #define tower2 26
  5. #define tower3 27
  6.  
  7. #define towerAmount 3
  8. #define delayBetweenTowers 500
  9. #define calibrateAmount 20
  10. #define calibrateTimeout 500
  11. #define meanError 30
  12. #define distanceFindTries 10
  13. #define distanceTriesTimeout 100
  14. #define timeDivider 100000
  15. #define timeout 0
  16.  
  17. //positions
  18. //tower 1
  19. #define x1 0.0
  20. #define y1 0.0
  21.  
  22. //tower 2
  23. #define x2 0.0
  24. #define y2 11.5
  25.  
  26. //tower 3
  27. #define x3 14.0
  28. #define y3 0.0
  29.  
  30. //position calculation
  31. #define A 2*(x1 - x2)
  32. #define B 2*(y1 - y2)
  33. #define C 2*(x2 - x3)
  34. #define D 2*(y2 - y3)
  35.  
  36. int towers[] = {tower1, tower2, tower3};
  37. int lastTowerNum = 0;
  38. int mean = 0;
  39. int currentSound = 0;
  40. int maxError = 0;
  41.  
  42. bool timerRunning = false;
  43.  
  44.  
  45. hw_timer_t* myTimer;
  46.  
  47. void IRAM_ATTR onTimer()
  48. {
  49.   timerStop(myTimer);
  50.   digitalWrite(towers[lastTowerNum], LOW);
  51.   Serial.println("Ended");
  52.   timerRunning = false;
  53. }
  54.  
  55.  
  56. void setup() {
  57.   Serial.begin(115200);
  58.  
  59.   pinMode(analogInput, INPUT);
  60.   pinMode(digitalInput, INPUT);
  61.   pinMode(tower1, OUTPUT);
  62.   pinMode(tower2, OUTPUT);
  63.   pinMode(tower3, OUTPUT);
  64.  
  65.   Serial.begin(115200);
  66.   Serial.println("Starting...");
  67.   delay(5000);
  68.   calibrateMean();
  69.  
  70.   myTimer = timerBegin(0, 2, true);
  71.   timerAttachInterrupt(myTimer, &onTimer, true);
  72.   timerAlarmWrite(myTimer, 160000000, true);
  73.   timerAlarmEnable(myTimer);
  74.   timerStop(myTimer);
  75.  
  76. }
  77.  
  78. void loop() {
  79.   // BT: Data send/receive via bluetooth
  80.   if (Serial.available()) {                                                               // BT: Checks if there are data from the serial monitor availabl
  81.     char msg = Serial.read();
  82.     Serial.println(msg);
  83.     if(msg == '1') // tenta achar as 3 distancias -> provavelmente melhor ir testando ele passo por passo antes de usar isso
  84.     {
  85.     // um possivel erro q eu acho q pode dar é que, se n me engano a comunicação por bluetooth pode ser meio lerda
  86.     // isso no geral pode causar um delay, mas como ele é comum pra todos, a solução rapida deve ser só "tempo -= delayMedio;" <---------------
  87.  
  88.       double distances[3];
  89.  
  90.       findThreeDistances(&distances[0]);
  91.       for(int i = 0; i < 3; i++)
  92.       {
  93.         Serial.print("Torre ");
  94.         Serial.print(i+1);
  95.         Serial.print(": ");
  96.         Serial.print(distances[i]);
  97.         Serial.println(" cm");
  98.       }
  99.  
  100.       double position[2];
  101.       calculatePosition(&distances[0], &position[0]);
  102.  
  103.       Serial.println("Positions: ");
  104.       Serial.print("X = ");
  105.       Serial.println(position[0]);
  106.       Serial.print("Y = ");
  107.       Serial.println(position[1]);
  108.       /*Serial.print("Torre ");
  109.       Serial.print(1);
  110.       Serial.print(": ");
  111.       Serial.print(findDistance(1));
  112.       Serial.println(" cm");*/
  113.  
  114.     }else if(msg == '2')
  115.     {
  116.       Serial.println("Starting calibration in 2 secs");
  117.       delay(2000);
  118.       calibrateMean();
  119.     }else Serial.println("sua mae");
  120.   }
  121.   /*if (SerialBT.available()) {                                                             // BT: Checks if there are data from the bluetooth available
  122.     Serial.write(SerialBT.read());                                                        // BT: Write the data to the serial monitor
  123.   }*/
  124.  
  125.   if(digitalRead(digitalInput))
  126.   {
  127.     currentSound = analogRead(analogInput);
  128.     hasSound(currentSound);
  129.     /*Serial.print("hasSound:");
  130.     Serial.println(hasSound(currentSound));
  131.     Serial.print("soundLevel:");
  132.     Serial.println(currentSound);
  133.     Serial.print("mean:");
  134.     Serial.println(mean);
  135.     Serial.print("Error:");
  136.     Serial.println(maxError);*/
  137.    
  138.   }
  139.  
  140.   delay(timeout);
  141. }
  142.  
  143. void findThreeDistances(double *distanceArray)// tenta achar as 3 distancias, recomendo tentr ir ao poucos, dps usar essa função
  144. {
  145.  
  146.   for(int i = 0; i < towerAmount; i++)    
  147.   {
  148.     distanceArray[i] = 0;
  149.     distanceArray[i] = findDistance(i);
  150.     delay(delayBetweenTowers);
  151.   }
  152. }
  153.  
  154. void refreshMean(int entry) // média dinamica, só entra aqui se não detectar um som feito por buzzer, ou seja ruido
  155. {
  156.   mean += entry;
  157.   mean /= 2;
  158. }
  159.  
  160. void calibrateMean() // necessário de silencio extremo nessa parte
  161. {                     //valores de maxError > 50 implicam q teve algum disturbio bem grande, recalibrar!
  162.   currentSound = analogRead(analogInput);
  163.   int pastSound = currentSound;
  164.   maxError = 0;
  165.   for(int i = 0; i < calibrateAmount; i++)
  166.   {
  167.     currentSound = analogRead(analogInput);
  168.     if(abs(currentSound - pastSound) > maxError) maxError = abs(currentSound - pastSound);
  169.     refreshMean(currentSound);
  170.     Serial.print("Calibrating - ");
  171.     Serial.print(i);
  172.     Serial.print("/");
  173.     Serial.println(calibrateAmount);
  174.     delay(calibrateTimeout);
  175.     pastSound = currentSound;
  176.   }
  177.   maxError += meanError;
  178.   Serial.println("Done!");
  179.   Serial.print("MaxError: ");
  180.   Serial.println(maxError);
  181. }
  182.  
  183. bool hasSound(int entry)
  184. {
  185.   if((entry > (mean + maxError)) || (entry < (mean - maxError))) return true;
  186.  
  187.   refreshMean(entry);
  188.   return false;
  189. }
  190.  
  191. double findDistance(int tower)
  192. {
  193.   uint64_t maxTime = 0;
  194.   int tries = 0;
  195.   Serial.println("Started finding distance.");
  196.   uint64_t previousTime;
  197.  
  198.   lastTowerNum = tower;
  199.   for(int i = 0; i < distanceFindTries; i++)
  200.   {
  201.     timerWrite(myTimer, 0);
  202.     timerAlarmEnable(myTimer);
  203.     timerRunning = true;
  204.  
  205.     digitalWrite(towers[tower], HIGH);
  206.     //while(!SerialBT.available());
  207.     timerStart(myTimer);
  208.    
  209.     while(!hasSound(analogRead(analogInput)) && timerRunning);
  210.  
  211.     if(timerRunning)
  212.     {
  213.       uint64_t endTime = timerReadMicros(myTimer);
  214.       timerStop(myTimer);
  215.       timerAlarmDisable(myTimer);
  216.      
  217.       timerRunning = false;
  218.       digitalWrite(towers[tower], LOW);
  219.       Serial.print("Ended with time variation: ");
  220.       Serial.println(endTime);
  221.       if(endTime > maxTime) maxTime = endTime;
  222.       if(i==0) previousTime = endTime;
  223.      
  224.  
  225.       if((abs(((int)(previousTime - endTime))) > 2000))
  226.       {
  227.         i =0;
  228.         maxTime = 0;
  229.         tries++;
  230.         Serial.println("Sus! -> reset");
  231.         if(tries >= 3)
  232.         {
  233.           Serial.println("kek");
  234.           return findDistanceMean(tower);
  235.         }
  236.       }
  237.  
  238.       previousTime = endTime;
  239.     }else{
  240.  
  241.       Serial.println("TimeOut");
  242.       i = 0;
  243.       maxTime = 0;
  244.     }
  245.     delay(distanceTriesTimeout);
  246.   }
  247.  
  248.  
  249.   if(microTimeToDistanceInCM(maxTime) < 11)
  250.   {
  251.     return microTimeToDistanceInCM(maxTime);
  252.     //return microTimeToDistanceInCM(2*maxTime);
  253.   }else return findDistanceMean(tower);
  254. }
  255.  
  256. double findDistanceMean(int tower)
  257. {
  258.   uint64_t avarageTime = 0;
  259.   int amountOfTimes = 0;
  260.   Serial.println("Started finding distance mean.");
  261.   uint64_t previousTime;
  262.  
  263.   lastTowerNum = tower;
  264.   for(int i = 0; i < 2*distanceFindTries; i++)
  265.   {
  266.     timerWrite(myTimer, 0);
  267.     timerAlarmEnable(myTimer);
  268.     timerRunning = true;
  269.  
  270.     digitalWrite(towers[tower], HIGH);
  271.     //while(!SerialBT.available());
  272.     timerStart(myTimer);
  273.    
  274.     while(!hasSound(analogRead(analogInput)) && timerRunning);
  275.  
  276.     if(timerRunning)
  277.     {
  278.       uint64_t endTime = timerReadMicros(myTimer);
  279.       timerStop(myTimer);
  280.       timerAlarmDisable(myTimer);
  281.      
  282.       timerRunning = false;
  283.       digitalWrite(towers[tower], LOW);
  284.       Serial.print("Ended with time variation: ");
  285.       Serial.println(endTime);
  286.  
  287.       avarageTime += endTime;
  288.       amountOfTimes ++;
  289.       //if(endTime > maxTime) maxTime = endTime;   // aqui
  290.       if(i==0) previousTime = endTime;
  291.      
  292.  
  293.       if((abs(((int)(previousTime - endTime))) > 5000))
  294.       {
  295.         i =0;
  296.         avarageTime = 0;
  297.         amountOfTimes = 0;
  298.  
  299.         Serial.println("Sus! -> reset");
  300.       }
  301.  
  302.       previousTime = endTime;
  303.     }else{
  304.  
  305.       Serial.println("TimeOut");
  306.       i = 0;
  307.       avarageTime = 0;
  308.       amountOfTimes = 0;
  309.     }
  310.     delay(distanceTriesTimeout);
  311.   }
  312.  
  313.   return microTimeToDistanceInCM(avarageTime/amountOfTimes);
  314.   //return microTimeToDistanceInCM(2*avarageTime/amountOfTimes);
  315. }
  316.  
  317. double microTimeToDistanceInCM(uint64_t timeVariation)
  318. {
  319.   return (timeVariation * 343.0)/timeDivider;
  320. }
  321.  
  322. void calculatePosition(double* distances, double* positions)
  323. {
  324.   double g1 = (distances[1]*distances[1]) + (x1*x1) + (y1*y1) - (distances[0]*distances[0]) - (x2*x2) - (y2*y2);
  325.   double g2 = (distances[2]*distances[2]) + (x2*x2) + (y2*y2) - (x3*x3) - (y3*y3) - (distances[1]*distances[1]);
  326.  
  327.   Serial.print("A = ");
  328.   Serial.println(A);
  329.  
  330.   Serial.print("B = ");
  331.   Serial.println(B);
  332.  
  333.   Serial.print("C = ");
  334.   Serial.println(C);
  335.  
  336.   Serial.print("D = ");
  337.   Serial.println(D);
  338.  
  339.   Serial.print("g1 = ");
  340.   Serial.println(g1);
  341.  
  342.   Serial.print("g2 = ");
  343.   Serial.println(g2);
  344.   positions[1] = ((A*g2) - (C*g1))/((A*D) - (B*C));
  345.   if(A != 0){
  346.     positions[0] = g1 - (B*positions[1]);
  347.     /*Serial.print("x1: ");
  348.     Serial.println(positions[0]);*/
  349.     positions[0] /= A;
  350.   }else
  351.   {
  352.     positions[0] = g2 - (D*positions[1]);
  353.     /*Serial.print("x1: ");
  354.     Serial.println(positions[0]);*/
  355.     positions[0] /= C;
  356.   }
  357.   //Serial.print("true x: ");
  358.   //Serial.println(positions[0]);
  359.  
  360. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement