ボール＆ビーム制御実験　

オブジェに飾っておくのに面白いものが簡単にできないか考えて，SI2012で見たものを更に簡単にして作ってみました． ボール＆ビームというのは制御分野ではメジャーな例題らしいですが，自分は専門外なので知りませんでした．

作ったのはこれ↓


Arduinoでサーボを制御，測距センサでピンポン玉との距離を計測，身近にあったスチロール板を適当にホットボンドでくっつけて工作．センサの高さと距離は輪ゴムで固定しているので可変できます． 本体製作時間は１時間半くらい，以下の動画のプログラムも含めて３－４時間で出来ました．

動画その１（ON-OFF制御）　
動画その２（比例制御）　
動画その３（比例と微分制御）　

完全に安定しないのは，測距センサの値がふらつくためです． 完全に安定するとボールが止まってしまって面白く無いので，あえてそのままにしています． 測距センサの計測周期に合わせて別周期で計測とスムージングした値を使うと多分もっと安定すると思います．

途中だけど，スケッチはこんな感じ

#include < Servo.h> 

const int analogInPin = A0; 
int servoCenter = 85;   
int sensorValue = 0;     
int posCenter=256;
float p=0, pp=0, i=0, d=0, pid=0;

Servo myservo; 
                 
void setup() 
{ 
  myservo.attach(9); 
  myservo.write(servoCenter);         
  delay(1000);
} 
 
void loop() 
{    
  sensorValue = analogRead(analogInPin;  
  pp=p;
  p=-(sensorValue-posCenter);
  i+=p;  // I制御は使ってません
  d=p-pp;
  pid=p*0.04+i*0+d*0.04;       // I制御は使ってません（係数０）
  myservo.write(servoCenter+pid);    
  delay(200);                     
} 

#include < Servo.h> 
#include < MsTimer2.h>
#define CENTERVAL 256          // センサ読みの線形化に使う
#define POS1 0.75              // 距離を２通り定義， 
#define POS2 1.3               // 中心が１なので，左右で少し離れた２点

const int analogInPin = A0;    // PSDセンサの出力をアナログポートにより計測
volatile int sensorValue = 0;  // PSDセンサの読み     
int servoCenter = 86;          // サーボが水平になる値． 
float posTarget=POS1;          // 目標位置
float posCenter=POS1;          // 制御対象を目標位置にゆっくり近づかせるための変数
int cnt=0;                     // 繰り返しの数のカウント 
float p=0, pp=0, i=0, d=0, pid=0;  // 制御パラメータ

Servo myservo;  
 
// --------- スムージング関数定義  --------------------
float smooth(float cData, float pData, float factor){
  return(cData*factor + pData*(1-factor));
}

// --------- PSDセンサ計測関数，タイマー割り込みで呼び出す  -------------
void measPSD(){
  sensorValue = smooth(analogRead(analogInPin),sensorValue,0.5);  // 計測値はスムージングして使う
}

// ----------------------- セットアップ  -------------------------------
void setup() 
{ 
  myservo.attach(9);   
  myservo.write(servoCenter);  // サーボを水平に          
  // Serial.begin(9600);       // 制御係数の確認のためにシリアル出力
  MsTimer2::set(25, measPSD);  // 25ms 周期でタイマー割込みをかけてPSD計測
  MsTimer2::start(); 
  delay(5000);                // 最初５秒サーボを水平に保つ
} 
// ----------------------- メインループ  ---------------------------------- 
void loop() 
{  
  cnt++; if (cnt>50){         // 200ms×50回で10秒毎に位置を切り替えるためのカウント処理
    cnt=0;
    if (posTarget==POS1) posTarget=POS2; else posTarget=POS1;     // 目標位置を切り替える
  }  
  posCenter+=(posTarget-posCenter)*0.15;    // 目標位置が急に変化するとバタツクのでなめらか処理
  pp=p;
  p=(float)CENTERVAL/(float)sensorValue - posCenter;  // 比例値
  i+=p;                                               // 積分値，今は未使用
  d=p-pp;                                             // 微分（差分）値
  pid=p*6+i*0+d*20;                                   
  myservo.write(servoCenter+pid);           
  
  // Serial.println(p);  
  delay(200);                  // 制御周期は200ms，早くするとバタついて球が跳ねる               
} 

​​ ESP32 Wifi Bluetooth開発ボード

​​ Arduino Nano Every

​​​ Raspberry Pi pico

​ FPGA XILINX Artix-7