Программа

Для управления сервомоторами робота используем стандартную библиотеку Servo. Движение сервомоторов на заданный угол осуществляется при помощи функции write. Для управления сервомоторов, необходимо подать на него ШИМ сигнал управления с помощью функции attach. А функция detach отключит сервомотор.

Ознакомиться с основами работы с сервомоторами в среде Arduino IDE можно в уроке управление сервоприводом.

Вращение сервомоторов

Напишем программу, которая сначала повернёт моторы в исходное положение — угол 90 градусов. Затем последовательно повращает все четыре мотора. В конце — отключит моторы.

#include "Servo.h"

// основание, первый узел, второй узел, захват
byte servoPins[4] = {11,10,9,5};
Servo srv[4];

void setup() {
    for(byte i=0; i<4; i++){
        srv[i].attach(servoPins[i]);
    }

    // сброс сервомоторов в центральное положение
    for(byte i=0; i<4; i++){
        srv[i].write(90);
    }

    delay(3000);

    // вращение в стороны
    for(byte i=0; i<4; i++){
        srv[i].write(80); // движение против часовой на 10 градусов
       delay(1000);
        srv[i].write(100); // движение по часовой на 10 градусов
        delay(1000);
        srv[i].write(90); // возвращение в исходное
        delay(1000);
    }

    delay(3000);

    // отключение сервомоторов
    for(byte i=0; i<4; i++){
        srv[i].detach();
    }
}

void loop() {
}

Движение по программе

Пусть теперь рука двигается по заданному алгоритму.

#include <Servo.h>

Servo js[4];

// основание, первый узел, второй узел, захват
int jpins[4] = {11,10,9,5};

// начальные углы
int angles[4] = {90,90,90,90};

// скорость
int sp = 10;

void movej( int jn, int a, int t ){
  int d, cnt, ca;
  int i = 0;
  ca = angles[jn];
  d = ca - a;
  cnt = abs(d);
  while( i<cnt ){
    js[jn].write(ca);
    if( d<0 )
      ca++;
    else
      ca--;
    i++;
    delay(t);
  }
  angles[jn] = ca;
}

void setup() {
  for(int i=0; i<4; i++)
    js[i].attach(jpins[i]);

  for(int i=0; i<4; i++){
    js[i].write(angles[i]);
    delay(500);
  }
}

void loop() {
  movej( 0, 130, sp ); // влево
  movej( 3, 90, sp ); // открыть клешню
  movej( 1, 20, sp ); // вперед
  movej( 2, 150, sp ); // вниз
  movej( 3, 130, sp ); // закрыть клешню

  movej( 2, 90, sp ); // вверх
  movej( 1, 110, sp ); // назад

  movej( 0, 30, sp ); // вправо

  movej( 1, 20, sp ); // вперед
  movej( 2, 150, sp ); // вних
  movej( 3, 90, sp ); // открыть клешню
  movej( 3, 130, sp ); // закрыть клешню

  movej( 2, 90, sp ); // вверх
  movej( 1, 110, sp ); // назад
}

Загружаем программу на Arduino и подаём питание.

Управление джойстиками

Напишем программу, которая будет анализировать сигнал с джойстиков и поворачивать соответствующие элементы робота.

#include <Servo.h>



#define MAX_J 768

#define MIN_J 256



uint8_t pos[4] = {90,90,90,90}; // начальные углы сервомоторов

// контакты джойстиков по порядку:
// левый вертикальный, левый горизонтальный
// правый горизонтальный, правый вертикальный
uint16_t pot_pins[4] = {A0,A1,A2,A3};  // lv, lh, rh, rv



uint8_t sn = 255;

int8_t sd = 0;

uint32_t st_next = 0;

uint16_t st_to = 10;


// таймаут отключения моторов

uint32_t idle_next = 0;

uint16_t idle_to = 500;



uint8_t servo_pins[4] = {11,10,9,5}; // контакты сервомоторов

uint8_t servo_inv[4] = {-1,-1,-1,1}; // инверсия управления
// предельные углы поворота

uint8_t servo_minmax[4][2] = {{45,135},{45,135},{45,135},{70,135}};



Servo arm[4];



void handleIdle(){

    uint32_t t = millis();

    if( idle_next && idle_next < t ){

        idle_next = 0;

        for( uint8_t i=0; i<4; i++ ){

            arm[i].detach();

        }

    }

}



void handleServo(){

    uint32_t t = millis();

    if( sn!=255 ){

        if( st_next && t < st_next )

            return;

        idle_next = t + idle_to;

        st_next = t + st_to;



        pos[sn] += servo_inv[sn]*sd;

        if( pos[sn]<servo_minmax[sn][0] )

            pos[sn] = servo_minmax[sn][0];

        else

        if( pos[sn]>servo_minmax[sn][1] )

            pos[sn] = servo_minmax[sn][1];



        if( !arm[sn].attached() )

            arm[sn].attach(servo_pins[sn]);



        arm[sn].write( pos[sn] );



    } else {

        arm[sn].detach();

        st_next = 0;

    }

}



void setup() {

    //Serial.begin(115200);

    for( uint8_t i=0; i<4; i++ ){

        pinMode(pot_pins[i], INPUT);

    }



    // движение в начальные позиции

    for( uint8_t i=0; i<4; i++ ){

        arm[i].attach(servo_pins[i]);

        arm[i].write(pos[i]);

        delay(1000);

        arm[i].detach();

    }

}



void loop() {

    sn = 255;

    uint16_t v;

    for( uint8_t i=0; i<4; i++ ){

        v = analogRead( pot_pins[i] );

        if (v > MAX_J || v < MIN_J){

            sn = i;

            sd = (v > MAX_J) ? 1 : -1;

        }

    }



    handleServo();

    handleIdle();

}