#ifndef _motor_h
#define _motor_h
#include "zf_common_headfile.h"

#define tim_0 4
#define tim_1 1
#define tim_2 100


extern float v1;
extern float v2;
extern float v3;
extern float v4;


extern float speed_pid1[3];
extern float speed_pid2[3];
extern float speed_pid3[3];
extern float speed_pid4[3];
extern int16 encoder_real[4];
extern int16 enlarge_k;
extern double speed_target[4];
extern double coord_now[3];
extern double coord_temp[2];
extern float location_kp;
extern float location_ki;
extern float location_kd;
extern float location_pid_out;
extern double x_speed;
extern double coord_sum[2];
extern double y_speed;
extern double error_add[3];
extern double coord_error0[3];
extern double coord_error1[3];
extern float pid_turn[3];
extern float angle;
extern double my_angle;
extern float tant;
extern float vector_x;
extern float vector_y;
extern float angle_z;
extern double aim_angle;
extern float turn_kp;
extern float turn_kd;
extern float angle_error0;
extern float angle_error1;
extern int16 angle_pwm;
extern float pic_location_kp;
extern float pic_location_ki;
extern float pic_location_kd;
extern float pic_vector_x;
extern float pic_vector_y;
extern float pic_angle_z;
extern double dis_x, dis_y;

int  myabs(int dat);
float limit(float x, int32 y);
double  myabs_double(double dat);

////
extern float yaw_angle;
extern int yaw_angle_flag;
///
double speed_pid_left_front(int16 real_s);

double speed_pid_right_front(int16 real_s);

double speed_pid_right_back(int16 real_s);

double speed_pid_left_back(int16 real_s);

void encoder(void);



void motor_control(void);

double get_aim_groy(double *aim_coord);

float location_x_pd(double *coord_now,double *coord_aim);

float location_y_pd(double *coord_now,double *coord_aim);

int16 turn_error_pid(double angle_turn);

float pic_x_pid(double error_X);

float pic_y_pid(double error_Y);

void pic_speed_coord(double Error_x,double Error_y,double er_angle);
void speed_coord_color();
void speed_coord_color_right();   
float turn_pid(float now_angle,float aim_angle);


void target_flash(double x,double y,double z);
void target_flash90(double x,double y,double z);
void target_flash180(double x,double y,double z);
void target_flash270(double x,double y,double z);
void pic_speed_find(void);

void speed_coord(double *coord_now,double *coord_aim);
void speed_coord_xun();
void encoder_flash(void);

void stop(void);

void back(void);
	
void test_angle(double angle);

void accgyro_to_coord(void);

double y_distance(float y);

double x_distance(float x);

void roate_shun();
void roate_ni();
void roate_tiaozhen();
extern int roate_tiaozhen_flag;
#endif