Сервоконтроллер на Ардуино Нано

Нет ответов
zhenious
Offline
Зарегистрирован: 03.07.2014

Решил сделать сервоконтроллер на Ардуино. 

Имеем:

1. Ардуино Нано.

2. Четыре серводвигателя SG90.

3. Две лапки из поликапролактона.

Отработаны два типа движения: лапки гребут одновременно и шагают попеременно.

Фото:

Скетч:

 int servo_1 = 2;
 int servo_2 = 3;
 int servo_3 = 4;
 int servo_4 = 5;
 long pastTime = 0;	
 
 int point_1 = 1550;
 int point_2 = 1500;
 int point_3 = 1500;
 int point_4 = 1550;
 
 int delta_1 = 200;
 int delta_2 = 200;
 int delta_3 = 200;
 int delta_4 = 200;
 
void setup()
{
  Serial.begin(9600);
  pinMode(servo_1, OUTPUT);
  pinMode(servo_2, OUTPUT);
  pinMode(servo_3, OUTPUT);
  pinMode(servo_4, OUTPUT);

}

void loop()
{
  /*
  //гребут одновременно
  writeToServo(point_1 - delta_1, point_2, point_3 + delta_3, point_4);
  delay(50);
  writeToServo(point_1, point_2 - delta_2, point_3, point_4 + delta_4);
  delay(50);
  writeToServo(point_1 + delta_1, point_2, point_3 - delta_3, point_4);
  delay(50);
  */
  
  //шагают попеременно
  writeToServo(point_1 - delta_1, point_2, point_3 - delta_3, point_4);
  delay(50);
  writeToServo(point_1, point_2 - delta_2, point_3, point_4);
  delay(50);
  writeToServo(point_1 + delta_1, point_2, point_3 + delta_3, point_4);
  delay(50);
  writeToServo(point_1, point_2, point_3, point_4 + delta_4);
  delay(50);
  
  
}

void writeToServo(int pos1, int pos2, int pos3, int pos4)
{
    for(int i=0; i<10; i++)
    {
        pastTime = micros();
        digitalWrite(servo_1, HIGH);
        delayMicroseconds(pos1); 
        digitalWrite(servo_1, LOW);

        digitalWrite(servo_2, HIGH);
        delayMicroseconds(pos2); 
        digitalWrite(servo_2, LOW);

        digitalWrite(servo_3, HIGH);
        delayMicroseconds(pos3); 
        digitalWrite(servo_3, LOW);

        digitalWrite(servo_4, HIGH);
        delayMicroseconds(pos4); 
        digitalWrite(servo_4, LOW);
        
        delayMicroseconds(10000);
        delayMicroseconds(3600 - (micros() - pastTime));
     }
     
}

Видео:

http://youtu.be/0RHPIA4_Mec

http://vk.com/video-73778892_171037183