Dynamixel Servo's feedback position control - Arduino Tutorial





Dynamixel Servo Position Feedback

The AX-12A servo actuator from Robotis is the most advanced actuator

IC 74LS241 Half-duplex 


Arduino Sketch

#include <AX12A.h>

#define POS_open_gate 100
#define POS_close_gate 410
#define DirectionPin (2u)
#define BaudRate  (57600ul)
#define ID (1u)

int led_red = 13;
int led_green = 12;

int pos = 0;

const int OpenbuttonPin = 8;
const int ClosebuttonPin = 9;

int OpenbuttonState = 0;         // current state of the button
int lastOpenbuttonState = 0;     // previous state of the button

int ClosebuttonState = 0;         // current state of the button
int lastClosebuttonState = 0;     // previous state of the button

void setup()
{
  pinMode( led_red , OUTPUT);
  pinMode( led_green , OUTPUT);
  
  pinMode( OpenbuttonPin , INPUT_PULLUP );
  pinMode( ClosebuttonPin , INPUT_PULLUP );
  
ax12a.begin(BaudRate, DirectionPin, &Serial);
  delay(1000);
  ax12a.move(1,100);
  delay(1000);

   BlinkLED();
   
}

void loop()
{
   OpenbuttonState  = digitalRead(OpenbuttonPin);
   ClosebuttonState = digitalRead(ClosebuttonPin);

  // Serial.println( OpenbuttonState );

   checkOpenButtonPress();
   checkCloseButtonPress();

     
pos = int(ax12a.readPosition(ID));

  if( ( POS_open_gate >= (pos-5) ) && ( POS_open_gate <= (pos+5) )){
      digitalWrite( led_red , LOW );
      digitalWrite( led_green , HIGH );
  }

  else if( ( POS_close_gate >= (pos-5) ) && ( POS_close_gate <= (pos+5) )){
      digitalWrite( led_red , HIGH );
      digitalWrite( led_green , LOW );
  }

}

void checkOpenButtonPress()
{
   // compare the OpenbuttonState to its previous state
  if (OpenbuttonState != lastOpenbuttonState) {
    // if the state has changed, Openrement the counter
    if (OpenbuttonState == LOW) {
      // if the current state is HIGH then the button went from off to on:
         ax12a.move(1,100);
    // delay(1000);  
      
    } else {
      // if the current state is LOW then the button went from on to off:
     // Serial.println("off");
    }
    // Delay a little bit to avoid bouncing
    delay(50);
  }
  // save the current state as the last state, for next time through the loop
  lastOpenbuttonState = OpenbuttonState;

}
void checkCloseButtonPress()
{
   // compare the OpenbuttonState to its previous state
  if (ClosebuttonState != lastClosebuttonState) {
    // if the state has changed, Openrement the counter
    if (ClosebuttonState == LOW) {
      // if the current state is HIGH then the button went from off to on:
      
     ax12a.move(1,410);
   //  delay(1000);
      
      
    } else {
      // if the current state is LOW then the button went from on to off:
     // Serial.println("off");
    }
    // Delay a little bit to avoid bouncing
    delay(50);
  }
  // save the current state as the last state, for next time through the loop
  lastClosebuttonState = ClosebuttonState;

}  
void BlinkLED()
{
  digitalWrite( led_red, HIGH);
  digitalWrite( led_green, HIGH);

  delay(500);

  digitalWrite( led_red, LOW);
  digitalWrite( led_green, LOW);

  delay(500);

  digitalWrite( led_red, HIGH);
  digitalWrite( led_green, HIGH);

  delay(500);

  digitalWrite( led_red, LOW);
  digitalWrite( led_green, LOW);

  
}

ความคิดเห็น