Header Ads

サーボに復元力を持たせる

, , , , ,
Arduinoでサーボモーターを自由自在にコントロールしたい。単にジョイスティックとつなぐだけでもいいがもっと動的にテンポよく動かしたい。人間は歩くとき足を上げたり下げたりはしていない。人間の頭が指示を出すのは足をあげるだけで、下げるのは重力と筋力が勝手に行っているように感じられる。そこでサーボモーターにも自動で戻るようにしてみたという実験。

サーボを３番ピンに、ジョイスティックを１番ピンに繋いである。ブロックはごちゃごちゃしすぎて画像を貼れないので、ファイル→RestoreServo.adpを参照してください。


一応コードも貼っておく。




#include <Servo.h>

int _ABVAR_1_Angle = 0 ;
Servo servo_pin_3;
int _ABVAR_2_Delta = 0 ;
int _ABVAR_3_JoyStick = 0 ;

void CheckAngle();
void SetServo();
void RestoreServo();
void SetJoystick();

void setup()
{
  servo_pin_3.attach(3);
  Serial.begin(9600);
  _ABVAR_1_Angle = 90 ;

  servo_pin_3.write( _ABVAR_1_Angle );

  _ABVAR_2_Delta = 0 ;

}

void loop()
{
  _ABVAR_3_JoyStick = analogRead(1) ;
  RestoreServo();
  SetJoystick();
  CheckAngle();
  delay( 40 );
}

void CheckAngle()
{
  if (( ( _ABVAR_1_Angle ) > ( 180 ) ))
  {
    _ABVAR_1_Angle = 180 ;
  }
  else
  {
    if (( ( _ABVAR_1_Angle ) < ( 0 ) ))
    {
      _ABVAR_1_Angle = 0 ;
    }
    else
    {
    }
  }
}

void SetServo()
{
  _ABVAR_1_Angle = ( _ABVAR_1_Angle + _ABVAR_2_Delta ) ;
  servo_pin_3.write( _ABVAR_1_Angle );
}

void SetJoystick()
{
  if (( ( _ABVAR_3_JoyStick ) < ( 128 ) ))
  {
    _ABVAR_2_Delta = -5 ;
    SetServo();
  }
  else
  {
    if (( ( _ABVAR_3_JoyStick ) < ( 256 ) ))
    {
      _ABVAR_2_Delta = -3 ;
      SetServo();
    }
    else
    {
      if (( ( _ABVAR_3_JoyStick ) < ( 384 ) ))
      {
        _ABVAR_2_Delta = -2 ;
        SetServo();
      }
      else
      {
        if (( ( _ABVAR_3_JoyStick ) < ( 500 ) ))
        {
          _ABVAR_2_Delta = -1 ;
          SetServo();
        }
        else
        {
          if (( ( _ABVAR_3_JoyStick ) < ( 520 ) ))
          {
            _ABVAR_2_Delta = 0 ;
            SetServo();
          }
          else
          {
            if (( ( _ABVAR_3_JoyStick ) < ( 640 ) ))
            {
              _ABVAR_2_Delta = 1 ;
              SetServo();
            }
            else
            {
              if (( ( _ABVAR_3_JoyStick ) < ( 768 ) ))
              {
                _ABVAR_2_Delta = 2 ;
                SetServo();
              }
              else
              {
                if (( ( _ABVAR_3_JoyStick ) < ( 896 ) ))
                {
                  _ABVAR_2_Delta = 3 ;
                  SetServo();
                }
                else
                {
                  if (( ( _ABVAR_3_JoyStick ) <= ( 1023 ) ))
                  {
                    _ABVAR_2_Delta = 5 ;
                    SetServo();
                  }
                  else
                  {
                    Serial.print("ERROR!JOYSTICK");
                    Serial.print(" ");
                    Serial.println();
                  }
                }
              }
            }
          }
        }
      }
    }
  }
}

void RestoreServo()
{
  if (( ( _ABVAR_1_Angle ) > ( 90 ) ))
  {
    _ABVAR_1_Angle = ( _ABVAR_1_Angle - 1 ) ;
  }
  else
  {
    if (( ( _ABVAR_1_Angle ) < ( 90 ) ))
    {
      _ABVAR_1_Angle = ( _ABVAR_1_Angle + 1 ) ;
    }
  }
}

Post a Comment

Powered by Blogger.