main.c

scara机器人的一种简单的实现方法

#include <graphics.h>
#include <process.h>
#include <conio.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include "m_menu.h"
#include "mpu.h"
#include "scara.h"
#include "mpu_func.h"

void   RS_MPU_PORT_INIT(void);                     //RS-232C for MPU
int    RS_SEND(int byte_data);                     //MPU send data
struct read_data RS_RECEIVE(void);                 //MPU receive data
void   RS_SCARA_PORT_INIT(void);                   //RS-232C for SCARA
int    RS_SCARA_SEND(int byte_data);               //send data to SCARA
int    RS_SCARA_RECEIVE(void);
void   Ini_menu(void);
void   Draw_robot(void);
int    Mouse_treat(void);
int    Quit(void);
void   RCCDsp(int x,int y,float xt,float yt,
	      int z,int A1,int A2,int Color,char *s);
void   drawbutton(int left_x,int top_y,int right_x,int bottom_y,
		  int flag,int col);

void   Error_treatment(int state);
struct print_ch ch_from_data(float data);
struct three_data Sample(void);
struct angle_distance Angle_distance_calculation(void);
int    Cp_adjustment(void);
int    Menu_from_key(void);
int    Func_step_movement(int stepped);

int    standard;

main ()
{
  int    mouse_state, chosed_button;
  int    step = 0, i;
  char   Key_value;


  RS_MPU_PORT_INIT();
  Ini_menu();
  Draw_robot();
  mouse_state = Mouse_treat();

  for (;;) {
    setviewport(0,0,624,464,1);

    if (chosed_button!=QUIT) {
      if (mouse_state==TRUE ) {
	     Show_Mouse();
	     chosed_button = Menu();
      }
      else  {
	     chosed_button = Menu_from_key();
	     RS_SCARA_PORT_INIT();
      }
      switch (chosed_button) {
	case STEP_MOVEMENT:
	  step++;
	  if (Func_step_movement(step)==FALSE) step--;
	  break;
	case CONTINUOUS_MOVEMENT:
     step++;
     for (i=step; i<12; i++) {
       if (Func_step_movement(i)==FALSE) i--;
     }
	  break;
	case PAUSE:
	  break;
	case STOP:
	  break;

	case QUIT:
	  Quit();
	  break;

  }}}

}

void treat(void)
{
  int    i, j;

  clrscr();
  graph_ini();

  Pscara.coordinate[0] = 400.;
  Pscara.coordinate[1] = 200.;
  Pscara.coordinate[2] = 0.;
  Pscara.coordinate[3] = 0.;
  Pscara.alpha         = 0.;

  line(0,400, 600, 400);
  line(10, 10, 10, 410);

  Pscara = Scara_theta_cal(Pscara);
//  printf("theta1 = %f ; theta2 = %f ; \n", Pscara.theta[0], Pscara.theta[1]);
  line(10, 400.5, 10+349.81*cos(Pscara.theta[0]),
		 400-349.81*sin(Pscara.theta[0]));
  line(10+349.81*cos(Pscara.theta[0]),400-349.81*sin(Pscara.theta[0]),
       10+349.81*cos(Pscara.theta[0])+249.42*cos(Pscara.theta[0]+Pscara.theta[1]),
      400-349.81*sin(Pscara.theta[0])-249.42*sin(Pscara.theta[0]+Pscara.theta[1]));
  Pscrewi = Screw_center_cal(Pscara, 0., 0., 0.);
//  printf("x = %f ; y = %f ; \n", Pscrewi.coordinate[0],
//				 Pscrewi.coordinate[1]);
  putpixel(10+Pscrewi.coordinate[0], 400-Pscrewi.coordinate[1], YELLOW);

//  for (i=0; i<360; i=i+5) {
    Pscrewi.alpha = 0.+ 24*3.1415926/180.;
    Pscara = Scara_point_cal(Pscrewi);
//    printf("x = %f ; y = %f ; \n", Pscara.coordinate[0], Pscara.coordinate[1]);
    putpixel(10+Pscara.coordinate[0], 400-Pscara.coordinate[1], WHITE);
//    printf("theta1 = %f ; theta2 = %f ; R = %f ; %d \n",
//	   Pscara.theta[0], Pscara.theta[1], Pscara.coordinate[3], i);

    ang_dis.angle = 0.10872665;
    Pscrewi.coordinate[3] = Pscara.coordinate[3];

    Func_step_movement(1);

/*  for (i=-10; i<11; i++) {
   for (j=10; j>-110; j--) {
    search_screw_center();
    Pscrewc = Screw_center_cal(Pscrewi, -55.0+delta_x, delta_y);

    Pscara = Scara_point_cal(Pscrewc);
//    printf("x = %f ; y = %f ; \n", Pscara.coordinate[0], Pscara.coordinate[1]);
    putpixel(10+Pscara.coordinate[0], 400-Pscara.coordinate[1], WHITE);

    Pscara = Scara_aligned_point_cal(Pscrewc, 5., ang_dis.angle);
//    printf("x = %f ; y = %f ; \n", Pscara.coordinate[0], Pscara.coordinate[1]);
    putpixel(10+Pscara.coordinate[0], 400-Pscara.coordinate[1], RED);
*/
    for (;;) {
      getch();  
      if (Func_step_movement(6)==TRUE) break;
    }
}