void XZ_Foot::set_direction(int d)
{
if(d==POSITIVE_DIR) {
set_pin_high(dir_pin_0, dir_connector_0);
set_pin_low(dir_pin_1, dir_connector_1);
} else {
set_pin_low(dir_pin_0, dir_connector_0);
set_pin_high(dir_pin_1, dir_connector_1);
}
}
static int
get_dht11_data(unsigned char *pbuf)
{
int i;
unsigned char DataTemp;
int ret = -EFAULT;
set_pin_high(); //拉高总线
udelay(40);
set_pin_low(); //拉低总线
msleep(20);
set_pin_high(); //拉高总线
udelay(40);
set_pin_input(); //设置输入模式
do
{
DataTemp = 10;
while (!(set_pin_get()) && DataTemp)
{
DataTemp--;
udelay(10);
}
if (!DataTemp)
{
break;
}
//
DataTemp = 10;
while ((set_pin_get()) && DataTemp)
{
DataTemp--;
udelay(10);
}
if (!DataTemp)
{
break;
}
//读取5字节
for (i = 0; i < 5; i++)
{
pbuf[i] = DHT11_read_byte();
}
//计算校验和
DataTemp = 0;
for (i = 0; i < 4; i++)
{
DataTemp += pbuf[i];
}
if (DataTemp != pbuf[4])
{
break;
}
ret = 5;
}
while (0);
set_pin_high(); //拉高总线
return ret;
}
int parser_servo_cmd(struct s_base_motion *cmd)
{
if(cmd == NULL) return -1;
if(cmd->left_action == START_ACTION) {
printf("Start servo!\n");
set_pin_high(servo_pin[0][0], servo_pin[0][1]);
} else {
printf("Stop servo!\n");
set_pin_low(servo_pin[0][0], servo_pin[0][1]);
}
return 0;
}
