int
devctl_bus_getstate(devctl_hdl_t dcp, uint_t *devstate)
{
int rv;
uint_t device_state;
if (devstate == NULL) {
errno = EINVAL;
return (-1);
}
rv = dc_cmd(DEVCTL_BUS_GETSTATE, 0, DCP(dcp), NULL,
(void *)&device_state);
if (rv == -1)
*devstate = 0;
else
*devstate = device_state;
if (_libdevice_debug)
(void) printf("devctl_bus_getstate: rv %d, state %x\n",
rv, *devstate);
return (rv);
}
int
devctl_pm_bus_teststrict(devctl_hdl_t dcp, uint_t *strict)
{
int rv;
uint_t strict_state;
if (strict == NULL) {
errno = EINVAL;
return (-1);
}
if (dcp == NULL || (DCP(dcp)->hdltype != DEVCTL_PM_BUS)) {
errno = EINVAL;
return (-1);
}
rv = dc_cmd(DEVCTL_PM_BUS_STRICT_TEST, 0, DCP(dcp), NULL,
(void *)&strict_state);
if (rv == -1)
*strict = 0;
else
*strict = strict_state;
if (_libdevice_debug)
(void) printf("devctl_pm_bus_teststrict: rv %d strict %x\n",
rv, *strict);
return (rv);
}
/*
* test pm busy state
*/
int
devctl_pm_testbusy(devctl_hdl_t dcp, uint_t *busystate)
{
int rv;
uint_t busy_state = 0;
if (busystate == NULL) {
errno = EINVAL;
return (-1);
}
if (dcp == NULL || (DCP(dcp)->hdltype != DEVCTL_PM_DEV &&
DCP(dcp)->hdltype != DEVCTL_PM_BUS)) {
errno = EINVAL;
return (-1);
}
rv = dc_cmd(DEVCTL_PM_BUSY_COMP_TEST, 0, DCP(dcp), NULL,
(void *)&busy_state);
if (rv == -1)
*busystate = 0;
else
*busystate = busy_state;
if (_libdevice_debug)
(void) printf("devctl_pm_bus_testbusy: rv %d busystate %x\n",
rv, *busystate);
return (rv);
}
int
devctl_bus_unquiesce(devctl_hdl_t dcp)
{
int rv;
rv = dc_cmd(DEVCTL_BUS_UNQUIESCE, 0, DCP(dcp), NULL, NULL);
if (_libdevice_debug)
(void) printf("devctl_bus_unquiesce: %d\n", rv);
return (rv);
}
int
devctl_bus_resetall(devctl_hdl_t dcp)
{
int rv;
rv = dc_cmd(DEVCTL_BUS_RESETALL, 0, DCP(dcp), NULL, NULL);
if (_libdevice_debug)
(void) printf("devctl_bus_resetall: %d\n", rv);
return (rv);
}
int
devctl_ap_unconfigure(devctl_hdl_t dcp, nvlist_t *ap_data)
{
int rv;
rv = dc_cmd(DEVCTL_AP_UNCONFIGURE, 0, DCP(dcp), ap_data, NULL);
if (_libdevice_debug)
(void) printf("devctl_ap_unconfigure: %d\n", rv);
return (rv);
}
int
devctl_ap_remove(devctl_hdl_t dcp, nvlist_t *ap_data)
{
int rv;
rv = dc_cmd(DEVCTL_AP_REMOVE, 0, DCP(dcp), ap_data, NULL);
if (_libdevice_debug)
(void) printf("devctl_ap_remove: %d\n", rv);
return (rv);
}
int
devctl_ap_insert(devctl_hdl_t dcp, nvlist_t *ap_data)
{
int rv;
rv = dc_cmd(DEVCTL_AP_INSERT, 0, DCP(dcp), ap_data, NULL);
if (_libdevice_debug)
(void) printf("devctl_ap_insert: %d\n", rv);
return (rv);
}
int
devctl_ap_disconnect(devctl_hdl_t dcp, nvlist_t *ap_data)
{
int rv;
rv = dc_cmd(DEVCTL_AP_DISCONNECT, 0, DCP(dcp), ap_data, NULL);
if (_libdevice_debug)
(void) printf("devctl_ap_disconnect: %d\n", rv);
return (rv);
}
int
devctl_device_reset(devctl_hdl_t dcp)
{
int rv;
rv = dc_cmd(DEVCTL_DEVICE_RESET, 0, DCP(dcp), NULL, NULL);
if (_libdevice_debug)
(void) printf("devctl_device_reset: %d\n", rv);
return (rv);
}
int
devctl_bus_unconfigure(devctl_hdl_t dcp)
{
int rv;
rv = dc_cmd(DEVCTL_BUS_UNCONFIGURE, 0, DCP(dcp), NULL, NULL);
if (_libdevice_debug)
(void) printf("devctl_bus_unconfigure: %d\n", rv);
return (rv);
}
/*
* devctl_bus_dev_create() - create a new child device
* Attempt to construct and attach a new child device below a
* bus nexus (dcp). The device is defined using the devctl_ddef_*()
* routines to specify the set of bus-specific properties required
* to initalize and attach the device.
*/
int
devctl_bus_dev_create(devctl_hdl_t dcp, devctl_ddef_t ddef_hdl,
uint_t flags, devctl_hdl_t *new_dcp)
{
char devname[MAXNAMELEN];
char devpath[MAXPATHLEN];
int rv = 0;
if (dcp == NULL || ddef_hdl == NULL) {
errno = EINVAL;
return (-1);
}
(void) memset(devname, 0, sizeof (devname));
rv = dc_cmd(DEVCTL_BUS_DEV_CREATE, flags, DCP(dcp),
(nvlist_t *)ddef_hdl, devname);
/*
* construct a device handle for the new device
*/
if ((rv == 0) && (new_dcp != NULL)) {
char *minorname, *lastslash;
(void) memset(devpath, 0, sizeof (devpath));
(void) strcat(devpath, DCP(dcp)->opath);
/*
* Take the pathname of the parent device, chop off
* any minor name info, and append the name@addr of
* the new child device.
* Call dc_mkhndl() with this constructed path and
* the CLONE handle type to create a new handle which
* references the new child device.
*/
lastslash = strrchr(devpath, '/');
if (*(lastslash + 1) == '\0') {
*lastslash = '\0';
} else {
if ((minorname = strchr(lastslash, ':')) != NULL)
*minorname = '\0';
}
(void) strcat(devpath, "/");
(void) strlcat(devpath, devname, MAXPATHLEN);
*new_dcp = dc_mkhndl(DEVCTL_CLONE, devpath, 0, dcp);
if (*new_dcp == NULL)
rv = -1;
}
return (rv);
}
/*
* issue DEVCTL_PM_BUS_NO_INVOL ioctl to set the NO_INVOL_FLAG
* flag: parent driver will mark itself idle twice in
* DDI_CTLOPS_DETACH(POST)
*/
int
devctl_pm_bus_no_invol(devctl_hdl_t dcp)
{
int rv;
if (dcp == NULL || DCP(dcp)->hdltype != DEVCTL_PM_BUS) {
errno = EINVAL;
return (-1);
}
rv = dc_cmd(DEVCTL_PM_BUS_NO_INVOL, 0, DCP(dcp), NULL, NULL);
if (_libdevice_debug)
(void) printf("devctl_pm_bus_no_invol: %d\n", rv);
return (rv);
}
/*
* issue DEVCTL_PM_NO_LOWER_POWER to clear the LOWER_POWER_FLAG
* flag: pm_lower_power() will not be called on device detach
*/
int
devctl_pm_device_no_lower_power(devctl_hdl_t dcp)
{
int rv;
if (dcp == NULL || DCP(dcp)->hdltype != DEVCTL_PM_DEV) {
errno = EINVAL;
return (-1);
}
rv = dc_cmd(DEVCTL_PM_NO_LOWER_POWER, 0, DCP(dcp), NULL, NULL);
if (_libdevice_debug)
(void) printf("devctl_pm_device_no_lower_power: %d\n", rv);
return (rv);
}
/*
* take the device OFFLINE and remove its dev_info node
*/
int
devctl_device_remove(devctl_hdl_t dcp)
{
int rv;
if (dcp == NULL || DCP(dcp)->hdltype != DEVCTL_DEVICE) {
errno = EINVAL;
return (-1);
}
rv = dc_cmd(DEVCTL_DEVICE_REMOVE, 0, DCP(dcp), NULL, NULL);
if (_libdevice_debug)
(void) printf("devctl_device_remove: %d\n", rv);
return (rv);
}
/*
* issue prom_printf() call
*/
int
devctl_pm_device_promprintf(devctl_hdl_t dcp)
{
int rv;
if (dcp == NULL || (DCP(dcp)->hdltype != DEVCTL_PM_DEV &&
DCP(dcp)->hdltype != DEVCTL_PM_BUS)) {
errno = EINVAL;
return (-1);
}
rv = dc_cmd(DEVCTL_PM_PROM_PRINTF, 0, DCP(dcp), NULL, NULL);
if (_libdevice_debug)
(void) printf("devctl_pm_device_promprintf: %d\n", rv);
return (rv);
}
/*
* set flag to fail DDI_SUSPEND
*/
int
devctl_pm_failsuspend(devctl_hdl_t dcp)
{
int rv;
if (dcp == NULL || (DCP(dcp)->hdltype != DEVCTL_PM_DEV &&
DCP(dcp)->hdltype != DEVCTL_PM_BUS)) {
errno = EINVAL;
return (-1);
}
rv = dc_cmd(DEVCTL_PM_FAIL_SUSPEND, 0, DCP(dcp), NULL, NULL);
if (_libdevice_debug)
(void) printf("devctl_pm_failsuspend: %d\n", rv);
return (rv);
}
/*
* Power up component 0, to level MAXPWR, via a pm_raise_power() call
*/
int
devctl_pm_raisepower(devctl_hdl_t dcp)
{
int rv;
if (dcp == NULL || (DCP(dcp)->hdltype != DEVCTL_PM_DEV &&
DCP(dcp)->hdltype != DEVCTL_PM_BUS)) {
errno = EINVAL;
return (-1);
}
rv = dc_cmd(DEVCTL_PM_RAISE_PWR, 0, DCP(dcp), NULL, NULL);
if (_libdevice_debug)
(void) printf("devctl_pm_raisepower: %d\n", rv);
return (rv);
}
/*
* set flag to power up the device via
* pm_power_has_changed() calls vs.
* pm_raise_power(), during DDI_RESUME
*/
int
devctl_pm_device_changeonresume(devctl_hdl_t dcp)
{
int rv;
if (dcp == NULL || (DCP(dcp)->hdltype != DEVCTL_PM_DEV &&
DCP(dcp)->hdltype != DEVCTL_PM_BUS)) {
errno = EINVAL;
return (-1);
}
rv = dc_cmd(DEVCTL_PM_PWR_HAS_CHANGED_ON_RESUME, 0,
DCP(dcp), NULL, NULL);
if (_libdevice_debug)
(void) printf("devctl_pm_device_changeonresume: %d\n", rv);
return (rv);
}
/*
* Power down component 0, to level 0, via a pm_change_power() call
*/
int
devctl_pm_changepowerlow(devctl_hdl_t dcp)
{
int rv;
if (dcp == NULL || (DCP(dcp)->hdltype != DEVCTL_PM_DEV &&
DCP(dcp)->hdltype != DEVCTL_PM_BUS)) {
errno = EINVAL;
return (-1);
}
rv = dc_cmd(DEVCTL_PM_CHANGE_PWR_LOW, 0, DCP(dcp), NULL, NULL);
if (_libdevice_debug)
(void) printf("devctl_pm_changepowerlow: %d\n", rv);
return (rv);
}
/*
* mark component 0 busy
*/
int
devctl_pm_busycomponent(devctl_hdl_t dcp)
{
int rv;
if (dcp == NULL || (DCP(dcp)->hdltype != DEVCTL_PM_DEV &&
DCP(dcp)->hdltype != DEVCTL_PM_BUS)) {
errno = EINVAL;
return (-1);
}
rv = dc_cmd(DEVCTL_PM_BUSY_COMP, 0, DCP(dcp), NULL, NULL);
if (_libdevice_debug)
(void) printf("devctl_pm_busycomponent: %d\n", rv);
return (rv);
}
int
devctl_ap_getstate(devctl_hdl_t dcp, nvlist_t *ap_data,
devctl_ap_state_t *apstate)
{
int rv;
devctl_ap_state_t ap_state;
rv = dc_cmd(DEVCTL_AP_GETSTATE, 0, DCP(dcp), ap_data,
(void *)&ap_state);
if (rv == -1)
(void) memset(apstate, 0, sizeof (struct devctl_ap_state));
else
*apstate = ap_state;
if (_libdevice_debug)
(void) printf("devctl_ap_getstate: %d\n", rv);
return (rv);
}
void run1()
{
u8 ack[2];
float old_angle,new_angle;
//printf("servlet 1");
LED1 = 0;
dc_moter_setspeed(2,65000);
dc_moter_setspeed(4,65000);
dc_cmd(2,1);
delay_ms(780);
dc_cmd(2,0);
dc_cmd(4,1);
delay_ms(780);
dc_cmd(4,0);
dc_cmd(2,1);
delay_ms(780);
dc_cmd(2,0);
dc_cmd(4,1);
delay_ms(780);
dc_cmd(4,0);
mpu6050_get();
old_angle = angle[1];
while(1)
{
delay_ms(10); //By QL 20150814
mpu6050_get();
new_angle = angle[1];
if((new_angle - old_angle)>0)
{
dc_cmd(2,1);
dc_cmd(4,0);
}
else if((new_angle - old_angle)<0)
{
dc_cmd(4,1);
dc_cmd(2,0);
}
if(new_angle > 15.5)
{
Beep(1);
dc_moter_setspeed(3,50000);
dc_cmd(1,0);
}
if(0 ==NRF24L01_RxPacket(ack))
{
Beep(0);
if(ack[0] == 's')
{
PDout(2)=1;
dc_cmd(2,0);
dc_cmd(4,0);
return;
}
}
}
}
void run4()
{
u8 run4_flag = 0 ,run4_flag1 = 0;
int x_count = 0,y_count = 0;
u8 i = 0;
float x_old_angle,x_new_angle,y_old_angle,y_new_angle;
//printf("servlet 4");
LED1 = 0;
dc_moter_setspeed(1,65000);
dc_moter_setspeed(2,65000);
dc_moter_setspeed(3,65000);
dc_moter_setspeed(4,65000);
for(i = 0; i < 20; i++)
{
mpu6050_get();
x_old_angle += angle[0];
y_old_angle += angle[1];
delay_ms(20);
}
x_old_angle = x_old_angle / i;
y_old_angle = y_old_angle / i;
while(1)
{
mpu6050_get();
delay_ms(25); //By QL 20150814
x_new_angle = angle[0];
y_new_angle = angle[1];
if((x_new_angle -x_old_angle)> 3)
{
x_count = 0;
dc_cmd(3,1);
dc_cmd(1,0);
}
else if((x_new_angle -x_old_angle)<-3)
{
x_count = 0;
dc_cmd(3,0);
dc_cmd(1,1);
}
if((y_new_angle-y_old_angle)> 3)
{
y_count = 0;
dc_cmd(4,1);
dc_cmd(2,0);
}
if((y_new_angle-y_old_angle) < -3)
{
y_count = 0;
dc_cmd(4,0);
dc_cmd(2,1);
}
if(((x_new_angle -x_old_angle)<=3) && (x_new_angle -x_old_angle)>=-3)
{
x_count ++;
if(x_count == 20)
{
run4_flag ++;
dc_cmd(3,0);
dc_cmd(1,0);
}
}
if(((y_new_angle -y_old_angle)<=3) && (y_new_angle -y_old_angle)>=-3)
{
y_count ++;
if(y_count == 20)
{
run4_flag1 ++;
dc_cmd(2,0);
dc_cmd(4,0);
}
}
if((run4_flag1 > 5)&&(run4_flag > 5))
{
PCout(0) = 1;
return ;
}
}
LED1 = 1;
}
void run5()
{
// u8 zql_ctrl=0; // By ZqL
u8 dr = 0; // 画圆的半径
int i;
int zql_x_pwm,zql_y_pwm;
int x_pwm,y_pwm;
float old_x_angle=0; //次角度值----X 方向
float old_y_angle=0; // 次角度值----y 方向
float new_x_angle,new_y_angle; // 次角度值 ---- 均值
int long_angle;
// Start
dc_moter_setspeed(1,MAX_PWM);
dc_moter_setspeed(2,MAX_PWM);
dc_moter_setspeed(3,MAX_PWM);
dc_moter_setspeed(4,MAX_PWM);
// y_pwm = MAX_PWM;
// x_pwm = MAX_PWM;
dc_cmd(FS_XZ,1); // STR Fan FS_XZ
dc_cmd(FS_YZ,0); // STR Fan FS_yZ
dc_cmd(FS_XF,0); // STp Fan FS_XF
dc_cmd(FS_YF,0); // STR Fan FS_YF
delay_ms(800);
dc_cmd(FS_XZ,0); // STR Fan FS_XZ
dc_cmd(FS_YZ,0); // STR Fan FS_yZ
dc_cmd(FS_XF,0); // STp Fan FS_XF
dc_cmd(FS_YF,0); // STR Fan FS_YF
dc_cmd(FS_XZ,0); // STR Fan FS_XZ
dc_cmd(FS_YZ,0); // STR Fan FS_yZ
dc_cmd(FS_XF,1); // STp Fan FS_XF
dc_cmd(FS_YF,0); // STR Fan FS_YF
delay_ms(800);
dc_cmd(FS_XZ,0); // STR Fan FS_XZ
dc_cmd(FS_YZ,0); // STR Fan FS_yZ
dc_cmd(FS_XF,0); // STp Fan FS_XF
dc_cmd(FS_YF,0); // STR Fan FS_YF
mpu6050_get();
old_x_angle = angle[0]; // x角度值
old_y_angle = angle[1]; // y角度值
delay_ms(10);
while (1)
{
mpu6050_get();
new_x_angle = angle[0]; // x角度值
new_y_angle = angle[1]; // y角度值
if((new_x_angle >= 0) && (new_y_angle >= 0)) // 第1相限,FS_XZ & FS_YF
{
dc_cmd(FS_XZ,1); // STR Fan FS_XZ
dc_cmd(FS_YZ,0); // STR Fan FS_yZ
dc_cmd(FS_XF,0); // STp Fan FS_XF
dc_cmd(FS_YF,1); // STR Fan FS_YF
//delay_ms(150);
}
else if((new_x_angle <= 0) && (new_y_angle >= 0)) // 第2相限,FS_XZ & FS_YZ
{
dc_cmd(FS_XZ,1); // STR Fan FS_XZ
dc_cmd(FS_YZ,1); // STR Fan FS_yZ
dc_cmd(FS_XF,0); // STp Fan FS_XF
dc_cmd(FS_YF,0); // STR Fan FS_YF
//delay_ms(150);
}
else if((new_x_angle <= 0) && (new_y_angle <= 0)) // 第3相限,FS_XF & FS_YZ
{
dc_cmd(FS_XZ,0); // STR Fan FS_XZ
dc_cmd(FS_YZ,1); // STR Fan FS_yZ
dc_cmd(FS_XF,1); // STp Fan FS_XF
dc_cmd(FS_YF,0); // STR Fan FS_YF
//delay_ms(150);
}
else if((new_x_angle >= 0) && (new_y_angle <= 0)) // 第4相限,FS_XF & FS_YF
{
dc_cmd(FS_XZ,0); // STR Fan FS_XZ
dc_cmd(FS_YZ,0); // STR Fan FS_yZ
dc_cmd(FS_XF,1); // STp Fan FS_XF
dc_cmd(FS_YF,1); // STR Fan FS_YF
//delay_ms(150);
}
}
}
void run2()
{
u8 ack[2];
float k;
u8 run_flag = 0; // nove dir flag
u8 tar2_flag;
float tar2_angle[]={9.9,10.54,11.18,11.82,12.46,13.09,13.73,14.30,14.97,15.59,16.22,16.81,17.40,18.08,18.66,19.2};
u16 pwm_data[]= {10000,10000,10000,12000,12000,12000,13500,13500,13500,14000,14000,14000,14800,14800,14800,14800};
float tar2_angle_data[100],tar2_angle_data_max;
float old_angle,new_angle;
float x_angle;
u8 rx_buf[10];
u8 long_data;
u8 tar2_check = 1;
int time_value;
dc_moter_setspeed(2,65000);
dc_moter_setspeed(4,65000);
while(1) //rev data
{
if(NRF24L01_RxPacket(rx_buf) == 0)
{
break;
}
}
LED1 = 0;
long_data = rx_buf[0]*10+rx_buf[1];
if(long_data % 2)
{
long_data --;
}
//printf("long_data %d",long_data);
x_angle = tar2_angle[(long_data-30)/2];
if(long_data >= 30 && long_data <35)
{
k = K30;
}
else if(long_data >= 35 && long_data <40)
{
k = K35;
}
else if(long_data >= 40 && long_data <45)
{
k = K40;
}
else if(long_data >= 45 && long_data <50)
{
k = K45;
}
else if(long_data >= 50 && long_data <55)
{
k = K50;
}
else if(long_data >= 55 && long_data <=60)
{
k = K55;
}
dc_cmd(2,1);
delay_ms(780);
dc_cmd(2,0);
dc_cmd(4,1);
delay_ms(780);
dc_cmd(4,0);
dc_cmd(2,1);
delay_ms(780);
dc_cmd(2,0);
dc_cmd(4,1);
delay_ms(780);
dc_cmd(4,0);
mpu6050_get();
old_angle = angle[1];
while(1)
{
delay_ms(10); //By QL 20150814
mpu6050_get();
new_angle = angle[1];
if((new_angle - old_angle)>0)
{
dc_cmd(2,1);
dc_cmd(4,0);
}
else if((new_angle - old_angle)<0)
{
dc_cmd(4,1);
dc_cmd(2,0);
}
if(new_angle > x_angle)
{
Beep(1);
dc_cmd(2,0);
break;
}
if(0 ==NRF24L01_RxPacket(ack))
{
Beep(0);
if(ack[0] == 's')
{
PDout(2)=1;
dc_cmd(2,0);
dc_cmd(4,0);
return;
}
}
}
time_value = (int)(k * KbuChang);
while(1) // 摆-----补偿循环
{
mpu6050_get();
old_angle = angle[1];
delay_ms(10); //By QL 20150814
mpu6050_get();
new_angle = angle[1];
if((new_angle - old_angle)>0 && (run_flag == 0))
{
dc_cmd(4,1); // start MT
delay_ms(time_value);
dc_cmd(4,0); // Stop MT
run_flag = 1;
}
if((new_angle - old_angle)<0)
{
run_flag = 0;
}
if(0 ==NRF24L01_RxPacket(ack))
{
Beep(0);
if(ack[0] == 's')
{
PDout(2)=1;
dc_cmd(2,0);
dc_cmd(4,0);
return;
}
}
}
}
void run3()
{
u8 tar3_time;
int i;
float old_angle,new_angle;
int x1_pwm_data[20] = {65000,65000,60000,60000,65000,48000,45000,40000,30000,0};
int y1_pwm_data[20] = {0,35000,40000,55000,65000,60000,60000,60000,60000,60000};
int x2_pwm_data[20] = {45000,48000,65000,60000,60000,65000,65000,65000};
int y2_pwm_data[20] = {60000,60000,65000,55000,40000,35000,32000,30000};
u8 dir = 0;
float x_max_angle,y_max_angle;
float x_angle;
float x_old_angle,x_new_angle,y_old_angle,y_new_angle;
u8 tar3_flag = 0;
int k = 0;
int x_pwm,y_pwm;
u8 rx_buf[10];
int long_angle;
//printf("servlet3");
while(1)
{
if(NRF24L01_RxPacket(rx_buf) == 0)
{
break;
}
}
LED1 = 0;
long_angle = rx_buf[0]*100+rx_buf[1]*10+rx_buf[2];
if(long_angle > 180)
{
long_angle = 360 -long_angle;
}
printf("long_angle %d",long_angle);
if(long_angle <= 90)
{
tar3_flag = 0; // xiang xian 1
}
else if(long_angle > 90)
{
tar3_flag = 1; // xiang xian 2
}
tar3_time = 12;
if(long_angle > 45 && long_angle < 135)
{
tar3_time = 6;
}
if(tar3_flag == 0) // area 1 process
{
x_pwm = x1_pwm_data[(long_angle)/10];
y_pwm = y1_pwm_data[(long_angle)/10];
//printf("x_pwm %d y_pwm %d",x_pwm,y_pwm);
dc_moter_setspeed(1,x_pwm);
dc_moter_setspeed(2,y_pwm);
dc_moter_setspeed(3,x_pwm);
dc_moter_setspeed(4,y_pwm);
i = 0;
while(1)
{
i++;
dc_cmd(1,0); // move to XX_1
dc_cmd(2,0);
dc_cmd(3,1);
dc_cmd(4,1);
delay_ms(800);
dc_cmd(3,0); // move to XX_3
dc_cmd(4,0);
dc_cmd(1,1);
dc_cmd(2,1);
delay_ms(800);
if((i*1.6)>tar3_time)
{
dc_cmd(3,0); // move to XX_3
dc_cmd(4,0);
dc_cmd(1,0);
dc_cmd(2,0);
PCout(0) = 1;
return ;
}
}
}
else if(tar3_flag == 1)
{
x_pwm = x2_pwm_data[(long_angle-100)/10];
y_pwm = y2_pwm_data[(long_angle-100)/10];
printf("x_pwm %d y_pwm %d",x_pwm,y_pwm);
dc_moter_setspeed(1,x_pwm);
dc_moter_setspeed(2,y_pwm);
dc_moter_setspeed(3,x_pwm);
dc_moter_setspeed(4,y_pwm);
i = 0;
while(1)
{
i++;
dc_cmd(1,0); // move to XX_1
dc_cmd(4,0);
dc_cmd(2,1);
dc_cmd(3,1);
delay_ms(800);
dc_cmd(2,0); // move to XX_3
dc_cmd(3,0);
dc_cmd(1,1);
dc_cmd(4,1);
delay_ms(800);
if((i*1.6)>tar3_time)
{
dc_cmd(3,0); // move to XX_3
dc_cmd(4,0);
dc_cmd(1,0);
dc_cmd(2,0);
PCout(0) = 1;
//tar3_time_flag = 0;
return ;
}
}
}
}
