u8 INPUT_ReadOne(u8 cs_num, u8 pin)
{
u8 tmp = 0;
assert_param(IS_INPUT_CS(cs_num));
assert_param(IS_INPUT_PIN(pin));
PDout(cs_num) = 0; //相应片选，低有效
tmp = PEin(pin);
PDout(cs_num) = 1;
return tmp;
}
u8 INPUT_Read(u8 cs_num)
{
u16 tmp = 0;
u8 input = 0xFF;
assert_param(IS_INPUT_CS(cs_num));
PDout(cs_num) = 0; //相应片选，低有效
tmp = GPIO_ReadInputData(GPIOE);
PDout(cs_num) = 1; //相应片选，低有效
input = tmp >> 8;
return input;
}
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 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;
}
}
}
}
