로봇암 소스코드

DS3218 서보모터  사용하기

서보모터사용하기.png

#include<Servo.h>

int servo_pin_x = 3;

int servo_pin_y =4;

int servo_pin_r = 6;

Servo  servo_01;

Servo  servo_02;

Servo  servo_03 ;

int  angle=0;

 int pin_x =A1;

int pin_y = A0;

int  pin_r = A2;

int  pos_x= 0;  pos_y =0  ;  pos_r =0;

int   cur_x = 90;   cur_y  = 90  ;   cur_r = 95;

void setup() {

Serial.begin (9600) ;

pinMode( pin_x , INPUT);

pinMode(  pin_y , INPUT) ;

pinMode ( pin_r ,  INPUT);

servo_01 . attach(servo_pin_x);

delay(10);

servo_02 . attach(servo_pin_y);

delay(10);

servo_03 . attach(servo_pin_r);

delay(10);

servo_01.write(cur_x);

delay(10);

servo_02.write(cur_y);

delay(10);

servo_03.write(cur_r);

delay(10);

}

void   loop() {

int  arm_motion_id = 0;

int  servo_delay_time  = 0;

pos_x = analogRead( pin_x);

pos_y = analogRead ( pin_y) ;

pos_r  = analogRead( pin_r) ;

if ( pos_r>750)

arm_motion_id  = 1;

else  if  ( pos_r < 250 )

 arm_motion_id  =  2 ;

if ( pos_y>750)

arm_motion_id  = 3;

else  if  ( pos_r < 250 )

 arm_motion_id  =  4 ;

if ( pos_x>750)

arm_motion_id  = 5;

else  if  ( pos_r < 250 )

 arm_motion_id  =  6 ;

switch(arm_motion_id) {

case1 :

    cur_r--;

  if ( cur_r  <44)

     cur_r = 44;

      servo_03.write(cur_r);

      servo_delay_time  = 50;

        break;

case2 :

    cur_r++;

  if ( cur_r  <95 )

     cur_r = 95 ;

      servo_03.write(cur_r);

      servo_delay_time  = 50;

        break;

case3 :

    cur_y--;

  if ( cur_y  <0 )

     cur_y = 0 ;

      servo_02.write(cur_y);

      servo_delay_time  = 50;

        break;

case4 :

    cur_y++  ;

  if ( cur_y  <180  )

     cur_y =  180 ;

      servo_02.write(cur_y);

      servo_delay_time  = 50;

        break;

case 5:

    cur_x--;

  if ( cur_x  <  0 )

     cur_x  = 0 ;

      servo_01.write(cur_x) ;

      servo_delay_time  = 50;

        break;

case6 :

    cur_x ++  ;

  if ( cur_x  < 180 )

     cur_x  = 180  ;

      servo_01.write(cur_x );

      servo_delay_time  = 50;

        break;

default :

break;

}

서보모터사용하기.png

0.01MB