LoginSignup
0
0
@koga1021

Drone+Arduino+Processing2

Last updated at Posted at 2023-07-16

#本格的なプログラム

x9c2chserial.ino
#define CS 2
#define UD 3
#define INC 4
#define CS1 5
#define UD1 6
#define INC1 7


#define CS2 8
#define UD2 9
#define INC2 10
#define CS3 11
#define UD3 12
#define INC3 13


int xcount = 0; /////x axis 0 - 0k ohm, 100 - 11k ohm
int ycount = 0; /////y axis

int xcount2 = 0; /////x axis 0 - 0k ohm, 100 - 11k ohm
int ycount2 = 0; /////y axis

boolean ser = false;
char tmp[20];
int x, tmpx;
int y, tmpy;
int x2, tmpx2;
int y2, tmpy2;

int ohm_max = 100; //// 44 - 5k ohm
int ohm_center1x = 40; /// 22 - 2.5k ohm
int ohm_center1y = 55; /// 22 - 2.5k ohm
int ohm_center2x = 40; /// 22 - 2.5k ohm
int ohm_center2y = 40; /// 22 - 2.5k ohm

int ohm_min = 0; /// 0 - 0k ohm

void setup()
{
  Serial.begin(115200);
  while (!Serial);
  // wait for Leonardo enumeration, others continue immediately

  // put your setup code here, to run once:
  //Serial.begin(9600);
  // Serial.println("X9C104 Digital POT");
  //////pot 1
  pinMode(CS, OUTPUT);
  pinMode(UD, OUTPUT);
  pinMode(INC, OUTPUT);
  //////pot 2
  pinMode(CS1, OUTPUT);
  pinMode(UD1, OUTPUT);
  pinMode(INC1, OUTPUT);
  ///////////////
  //////pot 3
  pinMode(CS2, OUTPUT);
  pinMode(UD2, OUTPUT);
  pinMode(INC2, OUTPUT);
  //////pot 4
  pinMode(CS3, OUTPUT);
  pinMode(UD3, OUTPUT);
  pinMode(INC3, OUTPUT);
  ///////////////

  // Serial.print("reset...");
  resetPot();
  delay(1000);
  resetPot2();
  delay(1000);
  // Serial.println("end");
}

void store()
{
  digitalWrite(INC, HIGH);
  digitalWrite(CS, HIGH);
  delay(50);
  digitalWrite(CS, LOW);
}

void FB(int c)
{
  digitalWrite(CS3, LOW);
  ////////////////ycount
  if (ycount2 < c)
  {
    digitalWrite(UD3, HIGH);
    for (int i = 0; i < c - ycount2; ++i)
    {
      digitalWrite(INC3, HIGH);
      delayMicroseconds(20); // pauses for 20 microseconds
      digitalWrite(INC3, LOW);
      delayMicroseconds(20); // pauses for 20 microseconds
    }
  } else {
    digitalWrite(UD3, LOW);
    for (int i = 0; i < ycount2 - c; ++i)
    {
      digitalWrite(INC3, HIGH);
      delayMicroseconds(20); // pauses for 20 microseconds
      digitalWrite(INC3, LOW);
      delayMicroseconds(20); // pauses for 20 microseconds
    }
  }
  ycount2 = c;
}

void RL(int c)
{
  digitalWrite(CS2, LOW);
  ////////////////
  if (xcount2 < c)
  {
    digitalWrite(UD2, HIGH);
    for (int i = 0; i < c - xcount2; ++i)
    {
      digitalWrite(INC2, HIGH);
      delayMicroseconds(20); // pauses for 20 microseconds
      digitalWrite(INC2, LOW);
      delayMicroseconds(20); // pauses for 20 microseconds
    }
  } else {
    digitalWrite(UD2, LOW);
    for (int i = 0; i < xcount2 - c; ++i)
    {
      digitalWrite(INC2, HIGH);
      delayMicroseconds(20); // pauses for 20 microseconds
      digitalWrite(INC2, LOW);
      delayMicroseconds(20); // pauses for 20 microseconds
    }
  }
  xcount2 = c;
}

void UPDOWN(int c)
{
  digitalWrite(CS1, LOW);
  ////////////////
  if (ycount < c)
  {
    digitalWrite(UD1, HIGH);
    for (int i = 0; i < c - ycount; ++i)
    {
      digitalWrite(INC1, HIGH);
      delayMicroseconds(20); // pauses for 20 microseconds
      digitalWrite(INC1, LOW);
      delayMicroseconds(20); // pauses for 20 microseconds
    }
  } else {
    digitalWrite(UD1, LOW);
    for (int i = 0; i < ycount - c; ++i)
    {
      digitalWrite(INC1, HIGH);
      delayMicroseconds(20); // pauses for 20 microseconds
      digitalWrite(INC1, LOW);
      delayMicroseconds(20); // pauses for 20 microseconds
    }
  }
  ycount = c;
}

void roll(int c)
{
  digitalWrite(CS, LOW);
  ////////////////ycount
  if (xcount < c)
  {
    digitalWrite(UD, HIGH);
    for (int i = 0; i < c - xcount; ++i)
    {
      digitalWrite(INC, HIGH);
      delayMicroseconds(20); // pauses for 20 microseconds
      digitalWrite(INC, LOW);
      delayMicroseconds(20); // pauses for 20 microseconds
    }
  } else {
    digitalWrite(UD, LOW);
    for (int i = 0; i < xcount - c; ++i)
    {
      digitalWrite(INC, HIGH);
      delayMicroseconds(20); // pauses for 20 microseconds
      digitalWrite(INC, LOW);
      delayMicroseconds(20); // pauses for 20 microseconds
    }
  }
  xcount = c;
}


void resetPot (void)
{
  digitalWrite(INC, HIGH);
  digitalWrite(CS, LOW);
  digitalWrite(UD, LOW);
  digitalWrite(INC1, HIGH);
  digitalWrite(CS1, LOW);
  digitalWrite(UD1, LOW);
  for (char i = 0; i < 100; i++) ////lowest
  {
    digitalWrite(INC, HIGH); // sets the pin on
    digitalWrite(INC1, HIGH); // sets the pin on
    delayMicroseconds(50); // pauses for 50 microseconds
    digitalWrite(INC, LOW); // sets the pin off
    digitalWrite(INC1, LOW); // sets the pin off
    delayMicroseconds(50); // pauses for 50 microseconds
  }
  //////////////////////////move to center R 11.35k ohm -> 2.5k ohm
  digitalWrite(UD, HIGH);
  digitalWrite(UD1, HIGH);

  for (char i = 0; i <= ohm_center1x; i++) ////center
  {
    digitalWrite(INC, HIGH); // sets the pin on
    delayMicroseconds(50); // pauses for 50 microseconds
    digitalWrite(INC, LOW); // sets the pin off
    delayMicroseconds(50); // pauses for 50 microseconds
  }


  for (char i = 0; i <= ohm_center1y; i++) ////center
  {
    digitalWrite(INC1, HIGH); // sets the pin on
    delayMicroseconds(50); // pauses for 50 microseconds
    digitalWrite(INC1, LOW); // sets the pin off
    delayMicroseconds(50); // pauses for 50 microseconds
  }

  digitalWrite(INC, HIGH);
  digitalWrite(CS, HIGH);
  digitalWrite(INC1, HIGH);
  digitalWrite(CS1, HIGH);
  delay(20);
  xcount = ohm_center1x;
  ycount = ohm_center1y;
}

void resetPot2 (void)
{
  digitalWrite(INC2, HIGH);
  digitalWrite(CS2, LOW);
  digitalWrite(UD2, LOW);
  digitalWrite(INC3, HIGH);
  digitalWrite(CS3, LOW);
  digitalWrite(UD3, LOW);
  for (char i = 0; i < 100; i++) ////lowest
  {
    digitalWrite(INC2, HIGH); // sets the pin on
    digitalWrite(INC3, HIGH); // sets the pin on
    delayMicroseconds(50); // pauses for 50 microseconds
    digitalWrite(INC2, LOW); // sets the pin off
    digitalWrite(INC3, LOW); // sets the pin off
    delayMicroseconds(50); // pauses for 50 microseconds
  }
  //////////////////////////move to center R 11.35k ohm -> 2.5k ohm
  digitalWrite(UD2, HIGH);
  digitalWrite(UD3, HIGH);

  for (char i = 0; i <= ohm_center2x; i++) ////center
  {
    digitalWrite(INC2, HIGH); // sets the pin on
    delayMicroseconds(50); // pauses for 50 microseconds
    digitalWrite(INC2, LOW); // sets the pin off
    delayMicroseconds(50); // pauses for 50 microseconds
  }

  for (char i = 0; i <= ohm_center2y; i++) ////center
  {
    digitalWrite(INC3, HIGH); // sets the pin on
    delayMicroseconds(50); // pauses for 50 microseconds
    digitalWrite(INC3, LOW); // sets the pin off
    delayMicroseconds(50); // pauses for 50 microseconds
  }

  digitalWrite(INC2, HIGH);
  digitalWrite(CS2, HIGH);
  digitalWrite(INC3, HIGH);
  digitalWrite(CS3, HIGH);
  delay(20);
  xcount2 = ohm_center2x;
  ycount2 = ohm_center2y;

  Serial.write(1);
  Serial.write(0);
  Serial.write(2);
  Serial.write(1);

}

/////////


void serialEvent()
{

  if ( Serial.available() > 0 && ser == false )
  {
    //合図用データを読み込みバッファを空にする
    for (int n = 15; n > 0; --n) tmp[n] = tmp[n - 1];
    tmp[0] = Serial.read();
    ///////////////!XYE
    ///////////////Xは0から100まで動く
    ///////////////Yは0から100まで動く
    if ( tmp[5] == 'h' && tmp[0] == 's' )
    {
      ////////////////////////////
      x  = tmp[3];
      y  = tmp[4];
      x2  = tmp[1];
      y2  = tmp[2];
      if (x < ohm_min) x = ohm_min;
      if (y < ohm_min) y = ohm_min;
      if (x > ohm_max) x = ohm_max;
      if (y > ohm_max) y = ohm_max;

      if (x2 < ohm_min) x2 = ohm_min;
      if (y2 < ohm_min) y2 = ohm_min;
      if (x2 > ohm_max) x2 = ohm_max;
      if (y2 > ohm_max) y2 = ohm_max;
      ////////////////////////////
      ser = true;
      Serial.write(x);
      Serial.write(y);
      Serial.write(x2);
      Serial.write(y2);
    }
  }

}


void loop()
{
  // put your main code here, to run repeatedly:
  //////////////////////move
  if ( ser )
  {
    ///////////////////////
    if ( x != tmpx ) roll( x );
    if ( y != tmpy ) UPDOWN( y );
    if ( x2 != tmpx2 ) RL( x2 );
    if ( y2 != tmpy2 ) FB( y2 );
    ///////////////////////
    tmpx = x;
    tmpy = y;
    tmpx2 = x2;
    tmpy2 = y2;
    ///////////////////////
    ser = false;
  }

}
drone.pde
/////////////////////////////////////////////受信に必要
import processing.serial.*;
Serial port;
/////////////////////////////////////////////
int xcount;
int ycount;
int xcount2;
int ycount2;


int inbuf[] = new int[1];
int buf[] = new int[4];
int cnt=0;

int ohm_max = 100; //// 44 - 4.98k = 5k ohm
int ohm_center = 50; /// 22 - 2.563k = 2.5k ohm
int ohm_min = 0; /// 0 - 0k ohm

void setup()
{
  println( Serial.list() );
  port = new Serial( this, "COM3", 115200 );

  xcount = ohm_center;
  ycount = ohm_center;

  xcount2 = ohm_center;
  ycount2 = ohm_center;
  
}

void draw()
{
}


void serialEvent(Serial port) {

  // port.readBytes(inbuf);

  inbuf[0] = port.read();

  buf[3] = buf[2];
  buf[2] = buf[1];
  buf[1] = buf[0];
  buf[0] = inbuf[0];
  cnt++;
  if (cnt>=4) {
    cnt=0;
    println(buf[0]+","+buf[1]+","+buf[2]+","+buf[3]);
  }
}

void keyPressed() 
{
  if (key == CODED) {
    if (keyCode == UP) 
    {
      ycount2 += 1;

      if (ycount2 > ohm_max) ycount2 = ohm_max;
      writee();
    }

    if (keyCode == DOWN) 
    {
      ycount2 -= 1;

      if (ycount2 < ohm_min) ycount2 = ohm_min;
      writee();
    }
    if (keyCode == RIGHT) 
    {
      xcount2 += 1;

      if (xcount2 > ohm_max) xcount2 = ohm_max;
      writee();
    }

    if (keyCode == LEFT) 
    {
      xcount2 -= 1;

      if (xcount2 < ohm_min) xcount2 = ohm_min;
      writee();
    }
  }

  if (key == 'w') {
    ycount += 1;

    if (ycount > ohm_max) ycount = ohm_max;
    writee();
  }

  if (key == 's') 
  {
    ycount -= 1;

    if (ycount < ohm_min) ycount = ohm_min;
    writee();
  }
  if (key == 'a') 
  {
    xcount -= 1;

    if (xcount < ohm_min) xcount = ohm_min;
    writee();
  }

  if (key == 'd') 
  {
    xcount += 1;

    if (xcount > ohm_max) xcount = ohm_max;
    writee();
  }
}

void writee() {
  port.write('h');
  port.write(ycount2);
  port.write(xcount2);
  port.write(ycount);
  port.write(xcount);
  port.write('s');
}


void keyReleased()
{
  xcount = ohm_center;
  ycount = ohm_center;
  xcount2 = ohm_center;
  ycount2 = ohm_center;
  writee();
}

キーボードのw,a,s,dが左スティック，各矢印が右スティックに対応するように設定を行い，キーボードでDroneをハッキングすることができる。
ここで注意点は，arduinoとprocessingに変数として定義してあるxcount,ycountとxcount2,ycount2はごちゃごちゃになっている．．．
おそらく
arduinoのxcountはprocessingのxcount2
arduinoのycountはprocessingのycount2
arduinoのxcount2はprocessingのxcount
arduinoのxcount2はprocessingのxcount
として定義してあるような．．．動いたのでそのままにして直していないです。

#PD制御を加えたプログラム

drone_pd.pde
import processing.video.*;
Capture cap;
/////////////////////////////////////////////受信に必要
import processing.serial.*;
Serial port;
/////////////////////////////////////////////
int xcount;
int ycount;
int xcount2;
int ycount2;

int pre_xcount;
int pre_ycount;
int pre_xcount2;
int pre_ycount2;


boolean flag = false;

int inbuf[] = new int[1];
int buf[] = new int[4];
int cnt=0;

int ohm_max = 100; //// 44 - 4.98k = 5k ohm
int ohm_min = 0; /// 0 - 0k ohm

//int ohm_center1 = 48; /// 22 - 2.563k = 2.5k ohm

  //自由製作課題drone
 int ohm_center1x = 48; /// 22 - 2.563k = 2.5k ohm  // RL&FB方向
 int ohm_center1y = 40; /// 22 - 2.563k = 2.5k ohm  // UPDOWN & Roll方向
 int ohm_center2x = 40; /// 22 - 2.563k = 2.5k ohm  // RL&FB方向
 int ohm_center2y = 40; /// 22 - 2.563k = 2.5k ohm  // UPDOWN & Roll方向
 
/*
//2つ目のDrone
int ohm_center1x = 58; /// 22 - 2.563k = 2.5k ohm  // RL&FB方向
int ohm_center1y = 58; /// 22 - 2.563k = 2.5k ohm  // UPDOWN & Roll方向
int ohm_center2x = 55; /// 22 - 2.563k = 2.5k ohm  // RL&FB方向
int ohm_center2y = 50; /// 22 - 2.563k = 2.5k ohm  // UPDOWN & Roll方向
*/

int x_center = 320;
int y_center = 240;

float kp=0.8;
float kd = 0.2;

float ytau_max = y_center;
float xtau_max = x_center;


int i, j;
PImage img;
int pixelColor; //ピクセルカラー
int R, G, B;    //赤、緑、青それぞれの値
int color_flg;


///////////////////クラスタ用
float area[];      //それぞれのクラスタ番号の面積
float area_x[];    //それぞれのクラスタ番号のxの合計
float area_y[];    //それぞれのクラスタ番号のyの合計
int map[][];       //クラスタリング[x][y]
int Line;          //行数、クラスタリングの番号

float a, b, c, d, e;
float minimum;
float dis[];       //前回の重心からの距離
int gg;            //今回の重心の位置(番号）
int I;             //何個あるか
float dis_min;     //最小距離


/////////////////////重心を求める
float m;           //個数
float ce_x, ce_y;  //重心の座標
float m1;           //個数
float ce_x1, ce_y1;  //重心の座標

int draw_cnt=0;

float pre_ce_x;
float pre_ce_y;

String path;
float aa= random(0, 10);

float tau_x, tau_y;
float delta_x, delta_y;
float div_x, div_y;
float Div_x, Div_y;

int minutee = minute();
int secondd = second();

void setup()
{
  size(640, 480);
  frameRate(30);

  ///////////////////カメラ関係
  cap = new Capture(this, 640, 480);
  cap.start();
  loadPixels();

  area = new float[2000];
  area_x = new float[2000];
  area_y = new float[2000];
  dis = new float[2000];
  map = new int[640][480]; 
  color_flg = 0;
  ce_x = 320;
  ce_y = 240;
  stroke(255);

  println( Serial.list() );
  port = new Serial( this, "COM11", 115200 );

  xcount = ohm_center1x;
  ycount = ohm_center1y;

  xcount2 = ohm_center2x;
  ycount2 = ohm_center2y;
   writee();
}

void draw()
{
  if (cap.available() == true)////読み込み成功 
  {
    cap.read();          //カメラからの画像を読み込む
    updatePixels();
    img = (PImage)cap;


    pre_ce_x = ce_x;
    pre_ce_y = ce_y;


    Clustering();
  }

  if (img != null)
  {
    stroke(255);
    image(img, 0, 0);
    if (m > 50)
    {
      line(ce_x, 0, ce_x, height);
      line(0, ce_y, width, ce_y);
    }
    /*
    if (m1 > 50)
     {
     stroke(0);
     line(ce_x1, 0, ce_x1, height);
     line(0, ce_y1, width, ce_y1);
     }*/
  }
  fill(255);
  textSize(15);
  text(" X = "+ce_x+"  "+" Y = "+ce_y+"  "+"m_white="+m, 20, 20);
  textSize(25);
  text("delta Y = "+(ce_y - height/2)+"delta X = "+(ce_x - width/2), 20, 90);
  text("xcount = "+xcount+"  ycount = "+ycount, 20, 120);
  text("tzu_x = "+tau_x+"  tau_y = "+tau_y, 20, 150);
  text("div_x = "+Div_x+"  div_y = "+Div_y, 20, 180);

  if (flag == true) {

    ohm_max = 90; //// 44 - 4.98k = 5k ohm
    ohm_min = 10; /// 0 - 0k ohm


    pre_xcount = xcount;
    pre_ycount = ycount;


    xcount = ohm_center1x;
    ycount = ohm_center1y;
    xcount2 = ohm_center2x;
    ycount2 = ohm_center2y;


    kp = 0.7;
    kd = 0.2;

    delta_y = ce_y - y_center;

    tau_y = 0;
    if (delta_y > 0) {
      tau_y = kp * map(delta_y, 0, ytau_max, 50, 100);
    } else {
      tau_y = kp * map(delta_y, 0, -ytau_max, 50, 0);
    }

    div_y = ce_y - pre_ce_y;
    Div_y = -kd * div_y;
    /*
      float Div_y = 0;
     if (div_y > 0) {
     Div_y = kd * map(div_y, 0, ytau_max, 50, 100);
     } else {
     Div_y = kd * map(div_y, 0, -ytau_max, 50, 0);
     }
     */

    ycount = (int)tau_y + (int)Div_y;

    if (ycount > ohm_max) ycount = ohm_max;
    if (ycount < ohm_min) ycount = ohm_min;

    delta_x = ce_x - x_center;

    tau_x = 0;

    if (delta_x > 0) {  
      tau_x = kp * map(-delta_x, 0, -xtau_max, 50, 0);
    } else {
      tau_x = kp * map(-delta_x, 0, xtau_max, 50, 100);
    }


    div_x = ce_x - pre_ce_x;
    Div_x = -kd * div_x;
    /*
      float Div_x = 0;
     if (div_x > 0) {
     Div_x = kd * map(div_x, 0, xtau_max, 50, 100);
     } else {
     Div_x = kd * map(div_x, 0, -xtau_max, 50, 0);
     }
     */
    xcount = (int)tau_x + (int)Div_x;
    if (xcount > ohm_max) xcount = ohm_max;
    if (xcount < ohm_min) xcount = ohm_min;

    /*
    if (ce_y - height/2 >= 0)
     {
     ycount += 2;
     
     if (ycount > ohm_max) ycount = ohm_max;
     //  writee();
     }
     if (ce_y - height/2 < 0)
     {
     ycount -= 2;
     
     if (ycount < ohm_min) ycount = ohm_min;
     //  writee();
     }
     if (ce_x - width/2 >= 0)
     {
     xcount += 2;
     
     if (xcount > ohm_max) xcount = ohm_max;
     //  writee();
     }
     if (ce_x - width/2 < 0)
     {
     xcount -= 2;
     
     if (xcount < ohm_min) xcount = ohm_min;
     //  writee();
     }
     */
    writee();
    /*
      if (draw_cnt ==0) {
     //path  = System.getProperty("user.home") + "/Desktop/processing_screenshot/drone/"+month()+"-"+day()+"-"+hour()+"-"+minutee+"-"+secondd+"-"+aa+"/screenshot" +draw_cnt + ".png";
     //save(path);
     writee();
     }*/


    draw_cnt++;
  }
}


void Clustering()
{
  //初期化
  Line = 1;
  a = 0;
  for ( i = 0; i < 2000; i++)
  {
    area[i] = 0;
    area_x[i] = 0;
    area_y[i] = 0;
    dis[i] = 1000;
  }

  for ( i = 0; i < img.height; i++)
  {
    for ( j = 0; j < img.width; j++)
    {
      //ビデオのピクセルを取り出す
      pixelColor = img.pixels[i*img.width + j];

      //赤、緑、青をそれぞれ抽出する(ビットシフト)
      R = (pixelColor >> 16) & 0xff;
      G = (pixelColor >> 8 ) & 0xff;
      B =  pixelColor        & 0xff;

      /////赤認証
      if (color_flg == 0)
      {
        if (R - G*1.2 - B*0.2 > 30)
        {
          img.pixels[i*img.width + j] = color(0, 255, 0);
          map[j][i] = Line;
          if (a == 0) Line++;
          a = 1;
        } else
        {
          //ウィンドウにピクセルを当てはめる
          img.pixels[i*img.width + j] = color(R, G, B);
          map[j][i] = 0;
          a = 0;
        }
      }

      /////緑認証
      if (color_flg == 1)
      {
        if (G - R*0.5 - B*0.5 > 30)
        {
          img.pixels[i*img.width + j] = color(255, 0, 0);
          map[j][i] = Line;
          if (a == 0) Line++;
          a = 1;
        } else
        {
          //ウィンドウにピクセルを当てはめる
          img.pixels[i*img.width + j] = color(R, G, B);
          map[j][i] = 0;
          a = 0;
        }
      }

      /////青認証
      if (color_flg == 2)
      {
        if (B - G*0.5 - R*0.5 > 30)
        {
          img.pixels[i*img.width + j] = color(255, 0, 0);
          map[j][i] = Line;
          if (a == 0) Line++;
          a = 1;
        } else
        {
          //ウィンドウにピクセルを当てはめる
          img.pixels[i*img.width + j] = color(R, G, B);
          map[j][i] = 0;
          a = 0;
        }
      }
    }
  }

  for (i = 1; i <= img.height-2; i++)
  {
    for (j = 1; j < img.width-2; j++)
    {
      a=map[j][i];//中心のpixel
      b=map[j][i-1];//上のpixel
      c=map[j-1][i];//左のpixel
      d=map[j][i+1];//下のpixel
      e=map[j+1][i];//右のpixel
      if (a != 0 &&
        b != 0 &&
        c != 0 &&
        d != 0 &&
        e != 0)
      {
        minimum = a;
        if (minimum >= b) minimum = b;
        if (minimum >= c) minimum = c;
        if (minimum >= d) minimum = d;
        if (minimum >= e) minimum = e;
        map[j][i]   = (int)minimum;
        map[j][i-1] = (int)minimum;
        map[j-1][i] = (int)minimum;
        map[j][i+1] = (int)minimum;
        map[j+1][i] = (int)minimum;
      }
    }
  }
  //////////////右下から左上へ
  for (i = img.height-2; i >= 1; i--)
  {
    for (j = img.width-2; j >= 1; j--)
    {
      a=map[j][i];//中心のpixel
      b=map[j][i-1];//上のpixel
      c=map[j-1][i];//左のpixel
      d=map[j][i+1];//下のpixel
      e=map[j+1][i];//右のpixel
      if (a != 0 &&
        b != 0 &&
        c != 0 &&
        d != 0 &&
        e != 0)
      {
        minimum = a;
        if (minimum >= b) minimum = b;
        if (minimum >= c) minimum = c;
        if (minimum >= d) minimum = d;
        if (minimum >= e) minimum = e;
        map[j][i]   = (int)minimum;
        map[j][i-1] = (int)minimum;
        map[j-1][i] = (int)minimum;
        map[j][i+1] = (int)minimum;
        map[j+1][i] = (int)minimum;
      }
    }
  }


  for ( i = 0; i < img.height; i++)        //それぞれの物体面積を求める
  {     
    for ( j = 0; j < img.width; j++)
    {
      if (map[j][i] >= 2000)
      {
        //エラー回避
      } else if (map[j][i] > 0)
      {
        area[map[j][i]]++;          //面積をインクリメントする
        area_x[map[j][i]] += j;     //x座標の合計を求める
        area_y[map[j][i]] += i;     //y座標の合計を求める
      }
    }
  }


  //////////////////////////////////////////////////////////
  /*
  for( i = 0;i < img.height; i++)
   {
   for( j = 0; j < img.width; j++)
   {
   if(map[j][i] > 0)
   {
   R = map[j][i]*7%16*15+100;
   G = map[j][i]*11%10*+100;
   B = map[j][i]*5%155+100;
   
   img.pixels[i*img.width + j] = color(R,G,B);
   }
   }
   }
   */
  ////////////////////////////////////////////////////////////////////////////////////
  //重心計算
  ////////////////////////////////////////////////////////////////////////////////////

  /////////////////面積が大きい順に並び替える
  for (i = 0; i < 2000; i++)
  {
    for ( j = 1+i; j < 2000; j++)
    {
      if (area[i] < area[j])
      {
        a = area[i];
        c = area_x[i];
        d = area_y[i];

        area[i] = area[j];
        area_x[i] = area_x[j];
        area_y[i] = area_y[j];

        area[j] = a;
        area_x[j] = c;
        area_y[j] = d;
      }
    }
  }
  /////////////////それぞれの重心の座標を求める
  for ( i = 0; i < 2000; i++ )
  {
    if (area[i] <= 50) 
    {
      I = i;
      break;
    }
    area_x[i] /= area[i];
    area_y[i] /= area[i];
  }

  ///////////////重心の距離を求める
  a = int(ce_x);
  b = int(ce_y);
  for ( i = 0; i <= I; i++)
  {
    c = area_x[i];
    d = area_y[i];
    dis[i] = int(dist(a, b, c, d));
  }


  ///////////////マウス操作によって割り込みをする
  if (mousePressed == true & (mouseButton == LEFT))
  { 
    for (i = 0; i <= I; i++)
    {
      a=mouseX;
      b=mouseY;
      c = area_x[i];
      d = area_y[i];
      dis[i]=int( dist(a, b, c, d) );
    }
  }

  ///////////////距離が短い方が重心
  dis_min = dis[0];
  gg = 0;
  for (i = 1; i < I; i++)
  {
    if (dis_min > dis[i])
    {
      dis_min = dis[i];  //最小
      gg = i;            //何番目か
    }
  }

  /////////////重心が決定
  ce_x = area_x[gg];
  ce_y = area_y[gg];
  m = area[gg];

  if (gg==0)
  {
    ce_x1 = area_x[gg+1];
    ce_y1 = area_y[gg+1];
    m1 = area[gg+1];
  } else
  {
    gg=0;
    ce_x1 = area_x[gg];
    ce_y1= area_y[gg];
    m1= area[gg];
  }
}

void serialEvent(Serial port) {

  // port.readBytes(inbuf);

  inbuf[0] = port.read();

  buf[3] = buf[2];
  buf[2] = buf[1];
  buf[1] = buf[0];
  buf[0] = inbuf[0];
  cnt++;
  if (cnt>=4) {
    cnt=0;
    println(buf[0]+","+buf[1]+","+buf[2]+","+buf[3]);
  }
}
/*
void keyPressed()
 {
 if (key == 'r')
 {
 color_flg = 0;
 }
 if (key == 'g')
 {
 color_flg = 1;
 }
 if (key == 'b')
 {
 color_flg = 2;
 }
 }
 */
void keyPressed() 
{
  if (key == CODED) {

    if (keyCode == UP) 
    {
      ycount += 2;

      if (ycount > ohm_max) ycount = ohm_max;
      writee();
    }

    if (keyCode == DOWN) 
    {
      ycount -= 2;

      if (ycount < ohm_min) ycount = ohm_min;
      writee();
    }
    if (keyCode == RIGHT) 
    {
      xcount += 2;

      if (xcount > ohm_max) xcount = ohm_max;
      writee();
    }

    if (keyCode == LEFT) 
    {
      xcount -= 2;

      if (xcount < ohm_min) xcount = ohm_min;
      writee();
    }
  }

  if (key == 'w') {
    ycount2 += 2;

    if (ycount2 > ohm_max) ycount2 = ohm_max;
    writee();
  }

  if (key == 's') 
  {
    ycount2 -= 2;

    if (ycount2 < ohm_min) ycount2 = ohm_min;
    writee();
  }
  if (key == 'a') 
  {
    xcount2 -= 2;

    if (xcount2 < ohm_min) xcount2 = ohm_min;
    writee();
  }

  if (key == 'd') 
  {
    xcount2 += 2;

    if (xcount2 > ohm_max) xcount2 = ohm_max;
    writee();
  }


  if (key == ENTER) {

    flag = true;
  }
}

void writee() {
  port.write('h');
  port.write(ycount2);
  port.write(xcount2);
  port.write(ycount);
  port.write(xcount);
  port.write('s');
}
/*
void start_drone() {
 port.write('s');
 port.write('d');
 port.write('e');
 }
 */
void keyReleased()
{
  xcount = ohm_center1x;
  ycount = ohm_center1y;
  xcount2 = ohm_center2x;
  ycount2 = ohm_center2y;
  writee();
}

クラスタリングを行い重心計算を行い，重心がカメラの中心地に居続けるようにPD制御を用いた。I制御はめんどくさくてやってないです。
パラメーターとかDの書き方など初心者ゆえ，しょうもない点が散見されると思いますがそれっぽく動きました．．．

個人的に重要な点は
ポテンションの中心値が50ではなく各素子によってまちまちであるという点です。しっかりオフセットを考慮する必要があるが自由度が少ない(x9c102なら1ステップ:10Ω)ので完全にオフセットを消し去ることは出来ません。
しかし，このような制御精度であっても提案手法でなら動かすことができればよい手法であると言えるのではないかと考えられる。

0
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
0
0