asked on

controlling a robot arm problem

Ok this is my first time ! Hi there !
I ve got a problem, I m working now on a experement of controlling Robot arm using image source(camera) that orders to the robert to move when it s sees an object, using the image signal processing. My problem is in the labeling phase, also I have written a program to calculate and draw the center of inertia just of 1 object, but i don t know how to do it for infinite numbers of objects.

#include <stdio.h>
#include <stdlib.h>
#include <conio.h>

#include "robot.h"
#include "img.h"

#define MODE 4
/********************/

extern BYTE *img_work2;
extern BYTE *img_work3;

/********************/
/*********************************************************************************

**********************************************************************************/
void disp_image_while_kbhit(BYTE *img, HDC *hDC); /*(サンプル)*/
void gray2(BYTE *img_src, HDC *hDC);
void get_rid_of_noise(BYTE *img_src, BYTE *img_tmp, HDC *hDC);
double scan_angle(BYTE *img, int g_x, int g_y);
void straight_line(BYTE *img, int g_x, int g_y, double theta, HDC *hDC);
double sqrt( double x );
double atan2( double x,double y);
double tan( double x);

void myfunc(BYTE *img_src , BYTE *img_tmp , HDC *hDC){
char motor;
int count;
int x1, y1, z1, x2, y2, z2 ;
int i, j, s=0;
int g_x, g_y, x_sum, y_sum, all_sum, threshold;
double theta;

switch(MODE){
/*-- rotate_longの動作確認(1) ----------------------------------------*/
case 1:
printf("rotate_longのmovement check(1)\n");
printf(" motor? ");
scanf("%c", &motor);

printf(" count? ");
scanf("%d", &count);

rotate_long( motor, count );
break;
/*-- rotate_long movement check(2) ----------------------------------------*/
case 2:
count = 120;
motor = 'B';
printf("rotate_long\n");
printf("モーター%cを%dだけ回転します。\n",motor,count);
rotate_long( motor, count );
getchar();
printf("モーター%cを%dだけ回転します。",motor,-count);
printf("元に戻れば、正しく動作しています。\n",motor,-count);
rotate_long( motor, -count );
getchar();
break;
/*-- move, move3 check-------------------------------------------*/
/* 自作してください */
case 3:
/* (1)coordonates*/
      printf("x1=\n"); scanf("%d",&x1);
      printf("x2=\n"); scanf("%d",&x2);
      printf("y1=\n"); scanf("%d",&y1);
      printf("y2=\n"); scanf("%d",&y2);
      printf("z1=\n"); scanf("%d",&z1);
      printf("z2=\n"); scanf("%d",&z2);

/* (2)move the arm from Home vision to position1,oepn and close the grip */
      move(XHOME, YHOME, ZHOME, x1, y1, z1);
      grip();
      ungrip();
/* (3)move the arm from cposition1 to position 2 */
      move(x1, y1, z1, x2, y2, z2);
      grip();
      ungrip();
/* (4)move the arm back to home vision from position 2、and close back the grip */
      move(x2, y2, z2, XHOME, YHOME, ZHOME);
      home('A');
/* (5)stop motors */
      stop_all();
break;
/*-- move, move3 -------------------------------------------*/

case 4:
/*display the image*/
disp_image_while_kbhit(img_src, hDC);
/*call the image source*/
get_image(img_src);
      /*gray the image*/
copy_image(img_src,img_tmp);
gray2(img_tmp,hDC);
      /*kill the noise*/
disp_gray_image(img_tmp,hDC);
get_rid_of_noise(img_tmp,img_work2,hDC);
copy_image(img_work2,img_tmp);

              show_monitor();
disp_image(img_tmp,hDC);
getchar();

            /*labeling*/

/* the center of inertia */
      /*Caluculation the Surface :white color = object*/
for(i=0;i<WIDTH;i++){
            for(j=0;j<HEIGHT;j++){
                   if(gray(img_tmp,j,i)==255){
                   s++;
                   }
             }
      }
/*get the coordonates of the center of inertia */
x_sum=0;
y_sum=0;
for(i=0;i<WIDTH;i++){
       for(j=0;j<HEIGHT;j++){
            if(gray(img_tmp,j,i)==255){
             x_sum+=i;
             y_sum+=j;
            }
      }
}
g_x=x_sum/s;
g_y=y_sum/s;
printf(" gx= %d, gy= %d\n", g_x, g_y);
      /*(draw)*/
for(i=0;i<480;i++){
      rgb(img_tmp,i,g_x,R)=255;
rgb(img_tmp,i,g_x,G)=0;
      rgb(img_tmp,i,g_x,B)=0;
}
for(j=0;j<640;j++){
      rgb(img_tmp,g_y,j,R)=255;
      rgb(img_tmp,g_y,j,G)=0;
      rgb(img_tmp,g_y,j,B)=0;
}
show_monitor();
disp_image(img_tmp,hDC);
getchar();
/* get the angle */
theta = scan_angle(img_tmp, g_x, g_y);
printf("Angle=(%lf)\n",theta);
straight_line(img_tmp, g_x, g_y, theta, hDC);
show_monitor();
disp_image(img_tmp,hDC);
getchar();
/*-------------------------------------------------------------------*/
break;

default:
printf("MODE=%dは有効ではありません。\n",MODE);
}
}

/*++ キーボード入力があるまでカメラの映像を表示する(サンプル) ++*/
void disp_image_while_kbhit(BYTE *img, HDC *hDC){
show_monitor();
while(!_kbhit()){
get_image(img);
disp_image(img,hDC);
}
_getch();
}

void gray2(BYTE *img_src, HDC *hDC){

int threshold;
       to_gray(img_src);
       from_gray(img_src);
       disp_image(img_src,hDC);
show_monitor();
       disp_histgram(img_src,hDC);
       printf("Threshold Range =\n");
       scanf("%d", &threshold);
       binarize(img_src,threshold);
}

void get_rid_of_noise(BYTE *img_src, BYTE *img_tmp, HDC *hDC){
      int k,l;
printf("Enter Dilation EffectNum\n");
scanf("%d",&k);
printf("Enter Erosion EffectNum\n");
scanf("%d",&l);
copy_image(img_src,img_tmp);
      erosion(img_tmp,k);
      dilation(img_tmp,l);
      dilation(img_tmp,l);
      erosion(img_tmp,k);
show_monitor();
disp_image(img_tmp,hDC);
getchar();
}

double scan_angle(BYTE *img, int g_x, int g_y){
double theta, lamda, s11=0, s12=0, s22=0;
int x, y;
for(x=0;x<WIDTH;x++){
       for(y=0;y<HEIGHT;y++){
              if(gray(img_work2, y, x)>100){
s11=(x-g_x)*(x-g_x);
s12=(x-g_x)*(y-g_y);
s22=(y-g_y)*(y-g_y);
            }
       }
       }
lamda=(s11+s22+sqrt(s11*s11+s22*s22-2*s11*s22+4*s12*s12))/2;
            theta=atan2(lamda-s11,s12);
return theta;
}

void straight_line(BYTE *img_tmp, int g_x, int g_y, double theta, HDC *hDC){
int x, y;
for(x=0;x<WIDTH;x++){
              y=(double)(x-g_x)*tan(theta)+g_y;
if(0>=y && y<HEIGHT){
rgb(img_tmp,y,x,R)=255;
      rgb(img_tmp,y,x,G)=0;
      rgb(img_tmp,y,x,B)=0;
       }
}
}
/*********************************************************************************
*********************************************************************************/
#define LIMIT 90

void rotate_long( char motor, int count ) {

int margin, dir;

if(count>0){
dir = 1;
}else{
dir = -1;
count =-count;
}

while( count > 0 ) {
/* (2) もし motor がストールしていたら, リターンする */
       if(stalled(motor)==1){
       return;
}

       margin = LIMIT-distance(motor);

      if(count<margin){
      rotate(motor,dir*count);
      count=LIMIT;
      return;
}
/* (5) margin が正の場合にのみ次のような処理を行う:
            margin の値だけ移動を指令し, count を margin だけ減らす */
       if(margin>0){
       rotate(motor,dir*margin);
       count=count-margin;
}
}

}

void      wait_DEF( void ) {
/* モータ D, E, F の動作終了待ちルーチン
(D, E, F のエラーレジスタのいずれかが正である間，
while ループを回り続ける) */
while(distance('D')>0 || distance('E')>0 || distance('F')>0){
}
}

void       move(int x1, int y1, int z1, int x2, int y2, int z2) {

double a[3],forward[3];

/* 手先を(x1, y1, z1) から (x2, y2, z2) へ移動させよ． */

/* (1)move from (x1, y1, z1) to (x2, y2, z2) */
      angle(x1,y1,z1,x2,y2,z2,a);
/* (2)配列 a に求めた回転角をステップ数に変換し,
      それぞれ配列 forward に収める */
forward[MOTOR_D]=a[MOTOR_D]/STEP_D;
forward[MOTOR_E]=a[MOTOR_E]/STEP_E;
forward[MOTOR_F]=a[MOTOR_F]/STEP_F;
/* (3)各関節を回転させる */
rotate_long('D',forward[MOTOR_D]);
rotate_long('E',forward[MOTOR_E]);
rotate_long('F',forward[MOTOR_F]);
/* (4)動作が終了するまで待つ */
while(distance('D')>0 || distance('E')>0 || distance('F')>0) {
}

}

void       move3(int x1, int y1, int z1, int x2, int y2, int z2) {
/* this part i want to move all the motors in the same time. so I wrote this following, not sure wether it s compatible or not */
int delta_x, delta_y, delta_z, dMAX;
delta_x = x2-x1;
delta_y = y2-y1;
delta_z = z2-z2;
dMAX = abs(delta_x);
if (dMAX<abs(delta_y) && dMAX>abs(delta_z))
dMAX = abs(delta_y);
if(dMAX<abs(delta_z) && dMAX>abs(delta_y))
dMAX = abs(delta_z);
if(dMAX<abs(delta_y) && dMAX<abs(delta_z) && abs(delta_z)<abs(delta_y))
dMAX = abs(delta_y);
if(dMAX<abs(delta_y) && dMAX<abs(delta_z) && abs(delta_y)<abs(delta_z))
dMAX = abs(delta_z);
move(x1, y1, z1, dMAX + x1, dMAX + y1, dMAX + z1);

}
/****************************************************************************************/

actually what i want from this program is to display something like the following :
http://www.convict.lu/Jeunes/5%20DOF%20Robot-arm.htm (Get_object_positionsRAC.vi.). so I want my program to display howmany objects r there, and their coordonates of center of inetia, the angle ...!
ah I think I want too much ! but man I have headach becoz of this program!