Advertisement
Guest User

Untitled

a guest
Jul 23rd, 2017
46
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 2.90 KB | None | 0 0
  1. #include <Servo.h>
  2.  
  3. Servo servoLeft;
  4. Servo servoRight;
  5.  
  6.  
  7. int globalSpeed = 5000;
  8. int minSpeed = 500;
  9. int maxSpeed = 5000;
  10. int minAngle = 10; // минимальный угол для серв
  11. int maxAngle = 170; // максимальный угол для серв
  12. int startLeftPos = 10; // стартовая позиция в градусах для левой сервы (должна быть в интервале (minAngle, maxAngle))
  13. int startRightPos = 10; // стартовая позиция в градусах для правой сервы (должна быть в интервале (minAngle, maxAngle))
  14. int currentLeftPos;
  15. int currentRightPos;
  16. int nextLeftPos;
  17. int nextRightPos;
  18. int leftDelay;
  19. int rightDelay;
  20. int signLeftPosDelta;
  21. int signRightPosDelta;
  22. unsigned long currentLeftTime;
  23. unsigned long currentRightTime;
  24.  
  25. void setup() {
  26. Serial.begin(9600);
  27. servoLeft.attach(10); // левая рука на 10 пине
  28. servoRight.attach(11); // правая рука на 11 пине
  29. randomSeed(analogRead(0)); // активируем генератор рандомных значений
  30. servoLeft.write(startLeftPos); // ставим левый серв в стартовую позицию
  31. servoRight.write(startRightPos); // ставим правый серв в стартовую позицию
  32. currentLeftPos = startLeftPos;
  33. currentRightPos = startRightPos;
  34. initLeft();
  35. initRight();
  36. }
  37.  
  38. void loop() {
  39. unsigned long time = millis();
  40. if (time - currentLeftTime > leftDelay) {
  41. currentLeftPos = max(minAngle, min(maxAngle, currentLeftPos + signLeftPosDelta));
  42. servoLeft.write(currentLeftPos);
  43. currentLeftTime = time;
  44. if (currentLeftPos == nextLeftPos) initLeft();
  45. }
  46. else Serial.print("leftDelay: " + leftDelay + " time: " + time + " currentLeftTime: " + currentLeftTime);
  47. if (time - currentRightTime > rightDelay) {
  48. currentRightPos = max(minAngle, min(maxAngle, currentRightPos + signRightPosDelta));
  49. servoRight.write(currentRightPos);
  50. currentRightTime = time;
  51. if (currentRightPos == nextRightPos) initRight();
  52. }
  53. }
  54.  
  55. int generateNextPos(int currentPos) {
  56. int newPos = random(minAngle, maxAngle + 1);
  57. while (newPos == currentPos) newPos = random(minAngle, maxAngle + 1);
  58. return newPos;
  59. }
  60.  
  61. void initLeft() {
  62. nextLeftPos = generateNextPos(currentLeftPos);
  63. int nextLeftSpeed = random(minSpeed, maxSpeed + 1);
  64. int leftPosDelta = currentLeftPos - nextLeftPos;
  65. leftDelay = nextLeftSpeed / abs(leftPosDelta);
  66. signLeftPosDelta = sign(leftPosDelta);
  67. currentLeftTime = millis();
  68. }
  69.  
  70. void initRight() {
  71. nextRightPos = generateNextPos(currentRightPos);
  72. int nextRightSpeed = random(minSpeed, maxSpeed + 1);
  73. int rightPosDelta = currentRightPos - nextRightPos;
  74. rightDelay = nextRightSpeed / abs(rightPosDelta);
  75. signRightPosDelta = sign(rightPosDelta);
  76. currentRightTime = millis();
  77. }
  78.  
  79. int sign(int x) {
  80. return x < 0 ? -1 : (x > 0 ? 1 : 0);
  81. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement