mpu_def.h

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

#include <graphics.h>
#include <process.h>
#include <conio.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <dos.h>


#define TRUE   0
#define FALSE  1
#define FAST   1
#define SLOW  -1
#define FINISH 0
#define CLOCKWISE        -1
#define COUNTER_CLOCKWISE 1
#define QUADRANT_1  0
#define QUADRANT_2  1
#define QUADRANT_3  2
#define QUADRANT_4  4
#define Initial_state_treatment		0
#define Move_to_initial_position        1
#define Turn_angle_alignment 		2
#define Angle_ditect    		3
#define Position_ditect		     	4
#define Change_bolt			5
#define Bolt_Insertion			6
#define Bolt_alignment			7
#define Turn_bolt    			8
#define Move_back_to_initial_position   9
#define Func_A                          65   // straight ditector
#define Func_B                          66   // slope ditector
#define Func_C                          67   // get data
#define Func_D                          68   // speed up cp
#define Func_E                          69   // slow down cp
#define Func_F                          70   // reset

#define pi  3.1415926

struct point{
  float data[4];
};

struct cal_point{
  float coordinate[4];
  float theta[2];
  float alpha;
};

struct three_data{
  int   d[3];
};

struct six_data{
  int   d[6];
};

struct print_ch{
  char  ch[25];
};

struct three_byte{
  int   data[3];
};

FILE *fp;
struct read_data {
  int d;
  int flag;
};

struct turn_data {       // alpha turnning data
  float ratio[10];
  float angle[10];
  int   flag;
};

struct angle_distance {  // screw's angle and distance to be detected
  float angle;
  float distance;
};

struct sc_move_data {    // screw center search state data
  int x_move_direction;  // sensor 1; 1 for X axis positive direction, -1 for negtive
  int x_move_state;
  int y_move_direction;  // sensor 2,3; 1 for Y axis positive direction, -1 for negtive
  int y_move_state;      // 1 for big step alignment, 0 for small
};

struct turn_data turn_angle;   // alpha should be found
struct angle_distance ang_dis; // theta and distance should be found
struct three_data mpu_slope_data[6], screw_center_data[2];
struct sc_move_data sc_search_state;
char   *print_str[9];
char   print_char[9][25];
struct point scara_move_point;
struct cal_point Pscara,  // scara end point position
		 Pscrewi, // screw center initial position
		 Pscrewc; // screw center current position after delta_x, y
float  max_delta_x=5.;   // allowed movement along x axis
float  max_delta_y=5.;   // allowed movement along y axis
float  max_z_coordinate=79.;
float  delta_x, delta_y;
float  screw_hight=30.;
float  center_distance=3.2;
float  inside_z=5.0;
float  move_step=0.25;
float  x_cross_point=0.;
float  y_cross_point=0.;
float  x_0 = 37., y_0 = 453., z_0 =55.;
int    screw_center_search_time = 0;
int    x_move_counter = 0;
int    y_move_counter = 0;
int    step = 0;
int    turn_angle_input_time = 0;