void ReadDataS(u8 id,NameType Name,u8 len,u16 *extractdata)
{
Dynamixel_Structure *data=NULL;
u8 temp=0,l;
u16 i=0;
for(l=0; l<len; l++)
{
temp=temp+Addr[Name+l][1]; //用k缓存 由寄存器个数计算出来的 数据个数
}
DataOPE(id,READ_DATA,Name,temp);
while(( ((data = Receive_Data(XL_TTL))==NULL||(*data).ID !=id))&&i<7000) //获取返回的信息 //获取返回的信息
i++;
temp=0; //缓存k 清零
if(((*data).ID==id)&&(data !=NULL))
{
for(l=0; l<len; l++)
{
if(Addr[Name+l][1] == 1) //判断寄存器长度
extractdata[l] =(*data).Data[temp++];
else
{
extractdata[l] = (u16)(((*data).Data[temp+1])<<8)+(u16)(((*data).Data[temp]));
temp=temp+2;
}
}
}
}
int Detect_CMD(void)
{
u16 i;
Dynamixel_Structure *data=NULL;
while((((data = Receive_Data(Upper_CMD))==NULL)||((*data).ID !=250)||(*data).Len!=1)&&(i<200))
i++;
if(i < 200) //超时判断
return (*data).Data[0];
else
return -1;
}
u16 ReadData(u8 id,NameType Name)
{
Dynamixel_Structure *data=NULL;
u16 status,i;
DataOPE(id,READ_DATA,Name,Addr[Name][1]);
while( (((data = Receive_Data(XL_TTL))==NULL)||((*data).ID !=id)||(((*data).Len-4)!=Addr[Name][1]))&&(i<7000))
i++; //获取返回的信息
if(i!=7000)
{
if(Addr[Name][1] == 1) //判断寄存器长度
{
status =(*data).Data[0];
}
else
{
status = ((*data).Data[1]<<8)+(*data).Data[0];
}
}
return status;
}
void WriteData(u8 id,NameType Name,int LEN,...)
{
u8 k=0,l=0;
u16 temp=0,i=0;
Dynamixel_Structure cmd,*data=NULL;
va_list ap; //声明 变参链表
va_start(ap,LEN); //指向 变参地址
cmd.ID = id;
cmd.Insruction = WRITE_DATA;
cmd.Data[k++]=Addr[Name][0]; //地址
cmd.Data[k++]=0x00;
for(l=0; l<LEN; l++)
{
if(Addr[Name+l][1]==1)
cmd.Data [k++] = (u8)((va_arg(ap, int))&0xff); //取出变参
else
{
temp =va_arg(ap, int);
cmd.Data [k++] = (u8)(temp&0xff); //取出变参
cmd.Data [k++] = (u8)((temp>>8)&0xff); //取出变参
}
}
va_end(ap);
cmd.Len = k; //数据个数+两个地址byte
Send_Data_TTL(cmd);
if(id==SPIN_FINGER)
{
if(i)
{
while((((data = Receive_Data(XL_TTL))==NULL)||((*data).ID !=id)||(*data).Data[0] !=0)&&(i<20000))
{
i++;
if(i==19999)
{
Send_Data_TTL(cmd);
i=0;
}
}
}
}
}
u8 DataOPE(u8 id,u8 instruction,NameType Name,u16 parm)
{
u8 k=0;
Dynamixel_Structure cmd;
cmd.ID = id;
switch(instruction)
{
case WRITE_DATA:
{
cmd.Insruction = WRITE_DATA;
cmd.Data[k++]= Addr[Name][0]; //低地址
cmd.Data[k++]=0x00;
if(Addr[Name][1]==1) //查询数据占用几个Byte
cmd.Data[k++] = (u8)parm; //写入参数
else
{
cmd.Data[k++] = (u8)(parm&0xff); //写入参数
cmd.Data[k++] = (u8)((parm>>8)&0xff); //写入参数
}
cmd.Len = k;
Send_Data_TTL(cmd);
break;
}
case READ_DATA:
{
cmd.Insruction = READ_DATA;
cmd.Data[k++]= Addr[Name][0]; //低地址
cmd.Data[k++]=0x00;
cmd.Data[k++]=(u8)parm; //此时 parm 代表读取长度 低长度
cmd.Data[k++]=0x00;
cmd.Len = k;
Send_Data_TTL(cmd);
break;
}
case PING:
{
Dynamixel_Structure cmd,*data=NULL;
cmd.ID = id;
cmd.Insruction = PING;
cmd.Len = 0x00;
Send_Data_TTL(cmd);
while( (data = Receive_Data(XL_TTL))==NULL||(*data).ID !=id); //获取返回的信息
if((*data).Data[0]) //判断Error Byte
return 0;
else
return 1; //在线 且无错误信息
}
case REG_WRITE:
{
cmd.Insruction = REG_WRITE;
cmd.Data[k++]= Addr[Name][0]; //低地址
cmd.Data[k++]=0x00;
if(Addr[Name][1]==1) //查询数据占用几个Byte
cmd.Data[k++] = (u8)parm; //写入参数
else
{
cmd.Data[k++] = (u8)(parm&0xff); //写入参数
cmd.Data[k++] = (u8)((parm>>8)&0xff); //写入参数
}
cmd.Len = k;
Send_Data_TTL(cmd);
break;
}
case ACTION:
{
cmd.ID = id;
cmd.Insruction = ACTION;
cmd.Len = 0x00;
Send_Data_TTL(cmd);
break;
}
case Reboot:
{
cmd.ID = id;
cmd.Insruction = Reboot;
cmd.Len = 0x00;
Send_Data_TTL(cmd);
break;
}
case FACTORY_RESET:
{
cmd.ID = id;
cmd.Insruction = FACTORY_RESET;
cmd.Data[0] = 0x02;
cmd.Len = 0x01;
Send_Data_TTL(cmd);
}
break;
default :
return 1;
}
return 1;
}
/**
* @brief wifi_init
* User API for wifi init
* @param None
* @retval None
*/
WiFi_Status_t wifi_init(wifi_config* config)
{
#ifndef WIFI_USE_VCOM
uint8_t tx_level;
#endif
WiFi_Status_t status = WiFi_MODULE_SUCCESS;
#if DEBUG_PRINT
printf("\r\nInitializing SPWF01SA1 Interface..\r\n");
#endif
WiFi_Module_Init();
#ifndef WIFI_USE_VCOM
wifi_wakeup(WIFI_TRUE);//Prevent from going to sleep during configuration
/* Soft reset the module */
wifi_reset();
/* Set localecho1 to 0*/
status = SET_Configuration_Value(LOCALECHO1, 0);
if(status != WiFi_MODULE_SUCCESS) return status;
/* Restore default setting*/
Reset_AT_CMD_Buffer();
sprintf((char*)(char*)WiFi_AT_Cmd_Buff,AT_RESTORE_DEFAULT_SETTING);
status = USART_Transmit_AT_Cmd(strlen((char*)WiFi_AT_Cmd_Buff));
if(status == WiFi_MODULE_SUCCESS)
{
status = USART_Receive_AT_Resp(Receive_AT_Cmd_Response);
if(status != WiFi_MODULE_SUCCESS) return status;
}
/* Switch on HW Flow Control*/
status = SET_Configuration_Value(CONSOLE1_HWFC, 1);
if(status != WiFi_MODULE_SUCCESS) return status;
if(config->wifi_baud_rate)
{
/* Set USART Speed*/
status = SET_Configuration_Value(CONSOLE1_SPEED, config->wifi_baud_rate);
if(status != WiFi_MODULE_SUCCESS) return status;
}
/* Set wifi_mode to idle*/
status = SET_Configuration_Value(WIFI_MODE, WiFi_IDLE_MODE);
if(status != WiFi_MODULE_SUCCESS) return status;
switch(config->ht_mode)
{
case WIFI_FALSE:
status = SET_Configuration_Value(WIFI_HT_MODE, 0);
if(status != WiFi_MODULE_SUCCESS) return status;
status = SET_Configuration_Addr(WIFI_OPR_RATE_MASK, "0x00003FCF");
if(status != WiFi_MODULE_SUCCESS) return status;
break;
case WIFI_TRUE:
status = SET_Configuration_Value(WIFI_HT_MODE, 1);
if(status != WiFi_MODULE_SUCCESS) return status;
status = SET_Configuration_Addr(WIFI_OPR_RATE_MASK, "0x003FFFCF");
if(status != WiFi_MODULE_SUCCESS) return status;
break;
default:
break;
}
switch(config->power)
{
case active:
status = SET_Configuration_Value(WIFI_POWERSAVE, 0);
if(status != WiFi_MODULE_SUCCESS) return status;
status = SET_Configuration_Value(WIFI_SLEEP_ENABLED, 0);
if(status != WiFi_MODULE_SUCCESS) return status;
break;
case reactive:
status = SET_Configuration_Value(WIFI_POWERSAVE, 1);
if(status != WiFi_MODULE_SUCCESS) return status;
status = SET_Configuration_Value(WIFI_OPERATIONAL_MODE, 11);
if(status != WiFi_MODULE_SUCCESS) return status;
status = SET_Configuration_Value(WIFI_BEACON_WAKEUP, 0);
if(status != WiFi_MODULE_SUCCESS) return status;
status = SET_Configuration_Value(WIFI_LISTEN_INTERVAL, 0);
if(status != WiFi_MODULE_SUCCESS) return status;
status = SET_Configuration_Value(WIFI_SLEEP_ENABLED, 0);
if(status != WiFi_MODULE_SUCCESS) return status;
break;
case sleep:
status = SET_Configuration_Value(WIFI_POWERSAVE, 1);
if(status != WiFi_MODULE_SUCCESS) return status;
status = SET_Configuration_Value(WIFI_OPERATIONAL_MODE, 12);
if(status != WiFi_MODULE_SUCCESS) return status;
status = SET_Configuration_Value(WIFI_BEACON_WAKEUP, 10);
if(status != WiFi_MODULE_SUCCESS) return status;
status = SET_Configuration_Value(WIFI_LISTEN_INTERVAL, 1);
if(status != WiFi_MODULE_SUCCESS) return status;
status = SET_Configuration_Value(WIFI_SLEEP_ENABLED, 1);
if(status != WiFi_MODULE_SUCCESS) return status;
break;
default:
break;
}
switch(config->power_level)
{
case low:
case medium:
case high:
case max:
tx_level=config->power_level*6;
status = SET_Configuration_Value(WIFI_TX_POWER, tx_level);
if(status != WiFi_MODULE_SUCCESS) return status;
break;
default:
break;
}
switch(config->dhcp)
{
case off:
case on:
case custom:
status = SET_Configuration_Value(IP_USE_DHCP_SERVER, config->dhcp);
if(status != WiFi_MODULE_SUCCESS) return status;
break;
default:
break;
}
/* Set IP address */
if(config->ip_addr)
{
status = SET_Configuration_Addr(WIFI_IP_ADDRESS, config->ip_addr);
if(status != WiFi_MODULE_SUCCESS) return status;
}
/* Set netmask address */
if(config->netmask_addr)
{
status = SET_Configuration_Addr(WIFI_IP_NETMASK, config->netmask_addr);
if(status != WiFi_MODULE_SUCCESS) return status;
}
/* Set default gateway address */
if(config->gateway_addr)
{
status = SET_Configuration_Addr(WIFI_IP_DEFAULT_GATEWAY, config->gateway_addr);
if(status != WiFi_MODULE_SUCCESS) return status;
}
/* Set dns address */
if(config->dns_addr)
{
status = SET_Configuration_Addr(WIFI_IP_DNS, config->dns_addr);
if(status != WiFi_MODULE_SUCCESS) return status;
}
/* Set hostname */
if(config->host_name)
{
status = SET_Configuration_Addr(WIFI_IP_HOSTNAME, config->host_name);
if(status != WiFi_MODULE_SUCCESS) return status;
}
if(config->ap_domain_name)
{
status = SET_Configuration_Addr(WIFI_IP_APDOMAINNAME, config->ap_domain_name);
if(status != WiFi_MODULE_SUCCESS) return status;
}
if(config->ap_config_page_name)
{
status = SET_Configuration_Addr(WIFI_IP_APREDIRECT, config->ap_config_page_name);
if(status != WiFi_MODULE_SUCCESS) return status;
}
if(config->http_timeout)
{
status = SET_Configuration_Value(WIFI_IP_HTTP_TIMEOUT, config->http_timeout*1000);
if(status != WiFi_MODULE_SUCCESS) return status;
}
if(config->dhcp_timeout)
{
status = SET_Configuration_Value(WIFI_IP_DHCP_TIMEOUT, config->dhcp_timeout);
if(status != WiFi_MODULE_SUCCESS) return status;
}
#ifdef MODULE_VERSION_SPWF01Sx_1y
Reset_AT_CMD_Buffer();
sprintf((char*)(char*)WiFi_AT_Cmd_Buff,AT_HTTPD, config->web_server);
status = USART_Transmit_AT_Cmd(strlen((char*)WiFi_AT_Cmd_Buff));
if(status == WiFi_MODULE_SUCCESS)
{
status = USART_Receive_AT_Resp(Receive_AT_Cmd_Response);
if(status != WiFi_MODULE_SUCCESS) return status;
}
#endif
/*AT+S.TLSCERT2=clean,all */
Reset_AT_CMD_Buffer();
sprintf((char*)(char*)WiFi_AT_Cmd_Buff,"AT+S.TLSCERT2=clean,all\r");
status = USART_Transmit_AT_Cmd(strlen((char*)WiFi_AT_Cmd_Buff));
if(status == WiFi_MODULE_SUCCESS)
{
status = USART_Receive_AT_Resp(Receive_AT_Cmd_Response);
if(status != WiFi_MODULE_SUCCESS) return status;
}
/* save current setting in flash */
Reset_AT_CMD_Buffer();
sprintf((char*)(char*)WiFi_AT_Cmd_Buff,AT_SAVE_CURRENT_SETTING);
status = USART_Transmit_AT_Cmd(strlen((char*)WiFi_AT_Cmd_Buff));
if(status == WiFi_MODULE_SUCCESS)
{
status = USART_Receive_AT_Resp(Receive_AT_Cmd_Response);
if(status != WiFi_MODULE_SUCCESS) return status;
}
if(config->wifi_baud_rate)
{
UART_Configuration(config->wifi_baud_rate);
Receive_Data();//Restart data reception
}
/* Soft reset the module, Do the second reset after setting all parameters and saving in flash */
wifi_reset();
wifi_wakeup(WIFI_FALSE);//De-assert wakeup signal (PC13) to allow sleep if enabled
#endif //WIFI_USE_VCOM
#if DEBUG_PRINT
printf("\r\nEnd of Initialization..\r\n");
#endif
return status;
}
