Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <Servo.h>
- Servo servoLeft;
- Servo servoRight;
- int globalSpeed = 5000;
- int minSpeed = 500;
- int maxSpeed = 5000;
- int minAngle = 10; // минимальный угол для серв
- int maxAngle = 170; // максимальный угол для серв
- int startLeftPos = 10; // стартовая позиция в градусах для левой сервы (должна быть в интервале (minAngle, maxAngle))
- int startRightPos = 10; // стартовая позиция в градусах для правой сервы (должна быть в интервале (minAngle, maxAngle))
- int currentLeftPos;
- int currentRightPos;
- int nextLeftPos;
- int nextRightPos;
- int leftDelay;
- int rightDelay;
- int signLeftPosDelta;
- int signRightPosDelta;
- unsigned long currentLeftTime;
- unsigned long currentRightTime;
- void setup() {
- Serial.begin(9600);
- servoLeft.attach(10); // левая рука на 10 пине
- servoRight.attach(11); // правая рука на 11 пине
- randomSeed(analogRead(0)); // активируем генератор рандомных значений
- servoLeft.write(startLeftPos); // ставим левый серв в стартовую позицию
- servoRight.write(startRightPos); // ставим правый серв в стартовую позицию
- currentLeftPos = startLeftPos;
- currentRightPos = startRightPos;
- initLeft();
- initRight();
- }
- void loop() {
- unsigned long time = millis();
- if (time - currentLeftTime > leftDelay) {
- currentLeftPos = max(minAngle, min(maxAngle, currentLeftPos + signLeftPosDelta));
- servoLeft.write(currentLeftPos);
- currentLeftTime = time;
- if (currentLeftPos == nextLeftPos) initLeft();
- }
- else Serial.print("leftDelay: " + leftDelay + " time: " + time + " currentLeftTime: " + currentLeftTime);
- if (time - currentRightTime > rightDelay) {
- currentRightPos = max(minAngle, min(maxAngle, currentRightPos + signRightPosDelta));
- servoRight.write(currentRightPos);
- currentRightTime = time;
- if (currentRightPos == nextRightPos) initRight();
- }
- }
- int generateNextPos(int currentPos) {
- int newPos = random(minAngle, maxAngle + 1);
- while (newPos == currentPos) newPos = random(minAngle, maxAngle + 1);
- return newPos;
- }
- void initLeft() {
- nextLeftPos = generateNextPos(currentLeftPos);
- int nextLeftSpeed = random(minSpeed, maxSpeed + 1);
- int leftPosDelta = currentLeftPos - nextLeftPos;
- leftDelay = nextLeftSpeed / abs(leftPosDelta);
- signLeftPosDelta = sign(leftPosDelta);
- currentLeftTime = millis();
- }
- void initRight() {
- nextRightPos = generateNextPos(currentRightPos);
- int nextRightSpeed = random(minSpeed, maxSpeed + 1);
- int rightPosDelta = currentRightPos - nextRightPos;
- rightDelay = nextRightSpeed / abs(rightPosDelta);
- signRightPosDelta = sign(rightPosDelta);
- currentRightTime = millis();
- }
- int sign(int x) {
- return x < 0 ? -1 : (x > 0 ? 1 : 0);
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement