/* InitToPose - gently moves Darwin into a ready position with speed 0x40
* Return - true if successful
* - false if unable to set torque, or if write fails
*/
bool DarwinController::InitToPose(){
unsigned char initpacket[108] = {0, };
unsigned char initparams[100] = {0, };
// Red eyes - Initialization in progress
int color = MakeColor(255, 0, 0);
unsigned char colorparams[2] = {GetLowByte(color), GetHighByte(color)};
MakePacket(initpacket, 0xC8, 2, 0x03, 0x1C, colorparams);
port.ClearPort();
port.WritePort(initpacket, 9);
// Torque enable
for(int IDnum = 0; IDnum < 21; IDnum++){
initparams[2*IDnum] = (unsigned char)(IDnum+1); // +1 because motors start at 1
initparams[2*IDnum+1] = 0x01;
}
unsigned char paramlength = 1;
unsigned char initnumparams = 40;
if(SyncWrite(initpacket, 0x18, initparams, initnumparams, paramlength) != 48){
printf("Failed Torque Enable in InitToPose!\n");
return false;
}
usleep(2000);
// Starting position and speed
for(int z = 0; z < 20; z++){
initparams[5*z] = z+1;
initparams[5*z+1] = 0x00;
initparams[5*z+2] = 0x08; // "zero" neutral position is 2048
initparams[5*z+3] = 0x40; // speed is 0x40
initparams[5*z+4] = 0x00;
}
if(SyncWrite(initpacket, 0x1E, initparams, 100, 4) != 108){
printf("Failed to init to pose\n");
return false;
}
sleep(1);
// Green eyes - Finished initializing
color = MakeColor(0, 255, 0);
initpacket[6] = GetLowByte(color);
initpacket[7] = GetHighByte(color);
colorparams[0] = GetLowByte(color);
colorparams[1] = GetHighByte(color);
MakePacket(initpacket, 0xC8, 2, 0x03, 0x1C, colorparams);
port.ClearPort();
port.WritePort(initpacket, 9);
return true;
}
int DarwinController::SetMoveSpeed(unsigned char joint_ID, int move_speed){
unsigned char speed_packet[9] = {0, };
unsigned char params[2] = {GetLowByte(move_speed), GetHighByte(move_speed)};
MakePacket(speed_packet, joint_ID, 2, WRITE, GOAL_POSITION_L, params);
unsigned char rxpacket[MAXNUM_RXPARAM] = {0, };
return ReadWrite(speed_packet, rxpacket);
}
void GdbServer::ReadyRead()
{
QTcpSocket * client = qobject_cast<QTcpSocket *>(sender());
socan = client;
input = client->readAll();
//special packet +- interrupt
if (input == "+")
return;
if (input == "-")
{
WARN(input);
WARN("send" + ans);
client->write(ans);
return;
}
if (input.at(0) == 3 && input.count() == 1)
{
stlink->CoreStop();
ans = "S05";
MakePacket(ans);
client->write(ans);
INFO("Interrupt");
return;
}
try {
//process packet
int ja = DecodePacket(input);
if (ja == 0)
processPacket(client,input);
else if (ja == 1)
{
//chvilu pockat
//client->write("-");
}
} catch (QString data)
{
WARN(data);
ans = "E00";
MakePacket(ans);
client->write(ans);
}
}
bool EmuTCPConnection::SendPacket(ServerPacket* pack, uint32 iDestination) {
if (!Connected())
return false;
eTCPMode tmp = GetMode();
if (tmp != modePacket && tmp != modeTransition)
return false;
LockMutex lock(&MState);
if (RemoteID)
return RelayLink->SendPacket(pack, RemoteID);
else if (pOldFormat) {
#if TCPN_LOG_PACKETS >= 1
if (pack && pack->opcode != 0) {
struct in_addr in;
in.s_addr = GetrIP();
CoutTimestamp(true);
std::cout << ": Logging outgoing TCP OldPacket. OPCode: 0x" << hex << setw(4) << setfill('0') << pack->opcode << dec << ", size: " << setw(5) << setfill(' ') << pack->size << " " << inet_ntoa(in) << ":" << GetrPort() << std::endl;
#if TCPN_LOG_PACKETS == 2
if (pack->size >= 32)
DumpPacket(pack->pBuffer, 32);
else
DumpPacket(pack);
#endif
#if TCPN_LOG_PACKETS >= 3
DumpPacket(pack);
#endif
}
#endif
SPackSendQueue* spsq = MakeOldPacket(pack);
ServerSendQueuePushEnd(spsq->buffer, spsq->size);
safe_delete_array(spsq);
}
else {
EmuTCPNetPacket_Struct* tnps = MakePacket(pack, iDestination);
if (tmp == modeTransition) {
InModeQueuePush(tnps);
}
else {
#if TCPN_LOG_PACKETS >= 1
if (pack && pack->opcode != 0) {
struct in_addr in;
in.s_addr = GetrIP();
CoutTimestamp(true);
std::cout << ": Logging outgoing TCP packet. OPCode: 0x" << hex << setw(4) << setfill('0') << pack->opcode << dec << ", size: " << setw(5) << setfill(' ') << pack->size << " " << inet_ntoa(in) << ":" << GetrPort() << std::endl;
#if TCPN_LOG_PACKETS == 2
if (pack->size >= 32)
DumpPacket(pack->pBuffer, 32);
else
DumpPacket(pack);
#endif
#if TCPN_LOG_PACKETS >= 3
DumpPacket(pack);
#endif
}
#endif
ServerSendQueuePushEnd((uchar**) &tnps, tnps->size);
}
}
return true;
}
int DarwinController::Set_PID_Gain(unsigned char joint_ID, unsigned char P_Value, unsigned char I_Value, unsigned char D_Value){
unsigned char PID_packet[10] = {0, };
unsigned char params[3] = {P_Value, I_Value, D_Value};
MakePacket(PID_packet, joint_ID, 1, WRITE, D_GAIN, params);
unsigned char rxpacket[MAXNUM_RXPARAM] = {0, };
return ReadWrite(PID_packet, rxpacket);
}
// Converts an angle given in degrees into motor ticks and sends write packet to motor
int DarwinController::SetJointAngle(unsigned char joint_ID, int goal_angle){
unsigned char angle_packet[9] = {0, };
int goal_ticks = goal_angle * 11.378; // 4096/360
unsigned char params[2] = {GetLowByte(goal_ticks), GetHighByte(goal_ticks)};
MakePacket(angle_packet, joint_ID, 2, WRITE, GOAL_POSITION_L, params);
unsigned char rxpacket[MAXNUM_RXPARAM] = {0, };
return ReadWrite(angle_packet, rxpacket);
}
void NetInputBroadcastMsg(NetInput *n, ServerMsg msg, const void *data)
{
if (!n->server)
{
return;
}
enet_host_broadcast(n->server, 0, MakePacket(msg, data));
enet_host_flush(n->server);
}
void SendPacket(unsigned char func, unsigned short param, const unsigned char *pData, unsigned int len)
{
unsigned char packet[500];
unsigned int nPacketLen;
nPacketLen = MakePacket(func, param, NodeEnv.Segment.GroupID, NodeEnv.Segment.NodeID, pData, len, packet);
if (nPacketLen < 256) {
BSP_Ser1_WriteBuffer(packet, nPacketLen-1);
SendDataCount++;
}
else {
BSP_Ser1_WriteBuffer(packet, nPacketLen);
}
}
void GdbServer::CoreHalted(uint32_t addr)
{
(void) addr;
//last command was continue
if (input == "c")
{
ans = "S05";
INFO("Breakpoint");
GDB_SEND(ans);
MakePacket(ans);
soc->write(ans);
}
}
QByteArray GdbServer::processBreakpointPacket(const QByteArray &data)
{
QByteArray ans;
params_t pars = ParseParams(data);
char z = data.at(0);
uint8_t type = pars[0].toInt();
uint32_t addr =(pars[1]).toInt(NULL,16);
//uint8_t kind = pars[2].toInt();
if (type != 1)
{
//not supported
MakePacket(ans);
return ans;
}
bool ok;
//insert breakpoint
if (z == 'Z')
{
ok = stlink->BreakpointWrite(addr);
}
//delete breakpoint
else if (z == 'z')
{
ok = stlink->BreakpointRemove(addr);
}
if (ok)
ans = "OK";
else
ans = "E00";
MakePacket(ans);
return ans;
}
OCmdTxInfo TCmdSender::Send(const ShpFCmdObject &shpFCmd,
const TMsgFrame &mf,
TErrList *pErr)
{
const IFCmdObject *pFCmdo=shpFCmd.Get();
if (!pFCmdo) return None;
TProtCmdPacket pkt;
if (!MakePacket(*pFCmdo, mf, pkt, pErr)) return None;
const OPktTxInfo opti=m_ps.Send(pkt, pErr);
if (!opti) return None;
const TCmdTxInfo cti(m_sng.Generate());
// TFCmdTx CmdTx(shpFCmd, cti); !!!!!
return cti;
}
// 使用串口协议发送数据
int CHvSerialLink::SendCmdData(const unsigned char bCmdId,
unsigned char* pbData,
unsigned int dwSendLen)
{
if (( pbData == NULL ) && (dwSendLen != 0))
{
return -1;
}
static char s_cmdbuf[MAX_PACK_LEN] = {FALG_BEGIN, FALG_NUMBER, 0};
unsigned int dwPacketLen = 0;
// 组包
MakePacket(bCmdId, FALG_NUMBER, (unsigned char*)pbData, dwSendLen, (unsigned char*)s_cmdbuf,
(unsigned int*)&dwPacketLen);
// 发送
return ((int)dwPacketLen == Send((unsigned char*)s_cmdbuf, dwPacketLen)) ? 0 : -1;
}
void PCPointShop::Init()
{
int i=0;
for(i=0;i<120;i++)
PC_Shop[i]=0x00;
for(i=0;i<MAX_PLAYERID-MIN_PLAYERID;i++)
{
pObjGS[i].PCInfoReceived=0;
pObjGS[i].PCInfoMod=0;
pObjGS[i].PCPlayerPoints=0;
}
ReadFile();
PacketSizes=MakePacket();
MaxPlayerPoints=32767;
PCPoint.AddWhenKillMobs=GetPrivateProfileInt("Character","AddPCPointsWhenKillMobs",1,"..\\OptionsData\\Options.ini");
PCPoint.OnlyForGM=GetPrivateProfileInt("Character","PCPointsOnlyForGM",1,"..\\OptionsData\\Options.ini");
}
void NetInputSendMsg(
NetInput *n, const int peerIndex, ServerMsg msg, const void *data)
{
if (!n->server)
{
return;
}
CASSERT(
peerIndex >= 0 && peerIndex < (int)n->peers.size,
"invalid peer index");
// Find the peer and send
for (int i = 0; i < (int)n->peers.size; i++)
{
ENetPeer **peer = CArrayGet(&n->peers, i);
if ((int)(*peer)->data == peerIndex)
{
enet_peer_send(*peer, 0, MakePacket(msg, data));
enet_host_flush(n->server);
return;
}
}
CASSERT(false, "Cannot find peer by id");
}
int main(int argc, char *argv[])
{
int sockfd, portno, n;
struct sockaddr_in serv_addr;
struct hostent *server;
char buf[256];
if (argc < 4) {
fprintf(stderr,"usage %s hostname port\n", argv[0]);
exit(0);
}
portno = atoi(argv[3]);
sockfd = socket(AF_INET, SOCK_STREAM, 0);
char *name=NULL;
int namelen = strlen(argv[1]);
name=(char*)malloc(namelen+1);
memset(name,0,namelen+1);
memcpy(name,argv[1],namelen);
name[namelen] = '\0';
//printf("name here is %s\n",name);
if (sockfd < 0)
{
error("ERROR opening socket\n");
exit(0);
}
server = gethostbyname(argv[2]);
if (server == NULL) {
fprintf(stderr,"ERROR, no such host\n");
exit(0);
}
bzero((char *) &serv_addr, sizeof(serv_addr));
serv_addr.sin_family = AF_INET;
bcopy((char *)server->h_addr,(char *)&serv_addr.sin_addr.s_addr,server->h_length);
serv_addr.sin_port = htons(portno);
if (connect(sockfd,(struct sockaddr *)&serv_addr,sizeof(serv_addr)) < 0)
{
error("ERROR connecting\n");
exit(0);
}
//int i,nbytes,j;
printf("\n********* Connected to the Server *********** \n");
//if connect successful send JOIN msg to server
short int typejoin =2; //set msg type as JOIN
char *buffer=NULL;
short int attype = 2; //attribute type=2 for username
n = MakePacket(typejoin,attype,&buffer,name);
//change the length of the data to be sent
//printf("Packet Done \n");
if (send(sockfd, buffer,n, 0) == -1) perror("error in sending JOIN MSG\n");
fd_set readSet,master_list_read;
FD_ZERO(&master_list_read);
FD_SET(STDIN, &master_list_read);//stdin manually trigger reading
FD_SET(sockfd, &master_list_read);//tcp socket
struct timeval tv;
int retval;
tv.tv_sec = 10;
tv.tv_usec = 0;
while(1)
{
char *buffer=NULL;
int n;
tv.tv_sec = 10;
readSet=master_list_read;
retval = select(sockfd+1, &readSet, NULL, NULL,&tv);
if (retval == -1)
{
perror("Error in select\n");
exit(4);
}
else if(retval==0)
{
if(!idlesend)
{
buffer=NULL;
short int typeidle =9; //set msg type as SEND
short int attuname = 2; //attribute type=2 for username
sprintf(buf,"%s",name);
n =MakePacket(typeidle,attuname,&buffer,name);
printf("!! Going Idle !!\n");
idlesend=true;
if (send(sockfd, buffer,n, 0) == -1)
perror("error in sending MSG to server\n");
//printf("*********SEND SUCCESSFUL******\n");
//free(buffer);
}
}
else
{
if(FD_ISSET(0, &readSet))
{
idlesend=false;
bzero(buf,256);
fgets(buf,256,stdin);
//buf[512]='\0';
/*for(i=0;i<5;i++)
printf("byte[%d]: %c \n",i,buf[i]);*/
short int typesend =4; //set msg type as SEND
short int attmsg = 4; //attribute type=4 for message
int n =MakePacket(typesend,attmsg,&buffer,buf);
if (send(sockfd, buffer,n, 0) == -1) perror("error in sending MSG to server\n");
//printf("*********SEND SUCCESSFUL******\n");
//free(buffer);
}
else if (FD_ISSET(sockfd, &readSet)) // receives data from server,print it on StdOutput
{
bzero(buf,256);
int nbytes = recv(sockfd, buf, 256,0);
if(nbytes<=0)
{
perror("recv error from server \n");
exit(0);
}
ExtractPacket(buf);
}
} //END OF ELSE
} //end of while
close(sockfd);
return 0;
}
QByteArray GdbServer::processQueryPacket(const QByteArray &data)
{
QByteArray ans;
if (data.contains( "qSupported:"))
{
ans.append("PacketSize=3fff");
ans.append(";qXfer:memory-map:read+");
stlink->BreakpointRemoveAll();
MakePacket(ans);
}
else if (data == ("qC") || data == "qSymbol::")
{
ans.append("OK");
MakePacket(ans);
}
else if (data == "qAttached" || data == "qOffsets")
{
MakePacket(ans);
}
else if (data == "qTStatus")
{
MakePacket(ans);
}
else if (data == ("qXfer:memory-map:read::0,fff"))
{
ans = stlink->GetMapFile();
ans.prepend('m');
MakePacket(ans);
}
else if (data.startsWith("qXfer:memory-map:read::27"))
{
ans = "l";
MakePacket(ans);
}
else if (data == "qfThreadInfo")
{
ans = "m1";
MakePacket(ans);
}
else if (data == "qsThreadInfo")
{
ans = "l";
MakePacket(ans);
}
else if (data.startsWith("qThreadExtraInfo"))
{
params_t pars = ParseParams(data);
int id = pars[1].toInt(NULL,16);
ans = getHandler().toLocal8Bit();
ans = ans.toHex();
MakePacket(ans);
}
else if (data.startsWith("qRcmd"))
{
bool fail = false;
params_t pars = ParseParams(data);
QByteArray arr = QByteArray::fromHex(pars[1]);
if (arr == "reset")
{
stlink->SysReset();
stlink->BreakpointRemoveAll();
}
else if (arr == "Reset")
{
stlink->SysReset();
}
else if (arr == "LOAD")
{
QFile moje(VeriFile);
if (moje.open(QFile::ReadOnly))
stlink->FlashWrite(FLASH_BASE,moje.readAll(),true);
}
else if (arr.startsWith("verify"))
{
QFile * file;
if (arr == "verify")
{
file = new QFile(VeriFile);
//file.setFileName(VeriFile);
}
else
{
arr.remove(0,7);
//file.setFileName(arr);
file = new QFile(arr);
}
int temp = 0;
if (file->open(QFile::ReadOnly))
{
bool ok = stlink->FlashVerify(file->readAll(),0);
if (ok)
temp = 0;
else
temp = 1;
file->close();
}
else
{
temp = 2;
}
QString text;
switch (temp)
{
case 0: text = "Verification OK"; break;
case 1: text = "Verification failed"; break;
case 2: text = "No file found "; break;
}
qDebug() << text;
delete file;
}
else if (arr == "erase")
{
stlink->FlashMassClear();
qDebug() << "Erased";
}
else if (arr == "PSP")
{
quint32 psp = stlink->ReadRegister(PSP);
QVector<quint32> vec;
stlink->UnstackContext(vec,psp);
QList<quint32> lst;
lst = vec.toList();
qDebug( ) << "PSP: " << QString("0x%1").arg(psp,0,16);
while(lst.count())
{
qDebug() << QString("0x%1").arg(lst.takeLast(),0,16);
}
switched = !switched;
qDebug() << switched;
}
else
{
qDebug() << "unknown remote command" << arr;
ans = "";
fail = true;
}
if (!fail)
ans = "OK";
MakePacket(ans);
}
return ans;
}
void GdbServer::processPacket(QTcpSocket *client,const QByteArray &data)
{
GDB_REC(data);
ans.clear();
if (data.startsWith('q'))
{
ans = processQueryPacket(data);
}
else if (data.startsWith("Z") || data.startsWith("z"))
{
ans = processBreakpointPacket(data);
} else
//init section
if (data.startsWith("Hg"))
{
thread_id = data.mid(2).toInt(NULL,16);
ans.append("OK");
MakePacket(ans);
}
else if (data.startsWith("H"))
{
ans.append("OK");
MakePacket(ans);
}
//if target halts
else if (data == ("?"))
{
//if halts
stlink->CoreStop();
ans.append("S05");
MakePacket(ans);
}
else if (data == "vCont?")
{
ans.clear();
MakePacket(ans);
}
//get general registers r0 - r15 (sp,lr,pc)
else if (data == "g")
{
QByteArray arr;
QVector<quint32> regs;
regs = stlink->ReadAllRegisters32();
if (switched)
regs = stlink->MergeContexts(regs,stlink->ReadRegister(PSP));
QStLink::Vector32toByteArray(arr,regs);
ans = arr.toHex();
MakePacket(ans);
}
//memory read
else if (data.startsWith("m"))
{
params_t pars = ParseParams(data);
uint32_t addr = (pars[0]).toLong(NULL,16);
if (addr == 0)
{
asm("nop");
}
uint32_t len = pars[1].toInt(NULL,16);
ans.clear();
stlink->ReadRam(addr,len,ans);
ans = ans.toHex();
MakePacket(ans);
}
//memory write
//data.startsWith("M") ||
else if ( data.startsWith("X"))
{
params_t pars = ParseParams(data);
uint32_t addr = (pars[0]).toLong(NULL,16);
uint32_t len = pars[1].toInt(NULL,16);
if (len > 0)
{
processEscapeChar(pars[2]);
QByteArray buf = pars[2];
stlink->WriteRam(addr,buf);
}
ans.clear();
ans = "OK";
MakePacket(ans);
}
//read register
else if (data.startsWith("p"))
{
params_t pars = ParseParams(data);
uint32_t idx = pars[0].toInt(NULL,16);
uint32_t reg;
reg = stlink->ReadRegister(idx);
ans.resize(4);
qToLittleEndian(reg,(uchar *)ans.data());
ans = ans.toHex();
MakePacket(ans);
}
//write register
else if (data.startsWith("P"))
{
params_t pars = ParseParams(data);
uint8_t reg = pars[0].toInt(NULL,16);
QByteArray temp = QByteArray::fromHex(pars[1]);
uint32_t val = qFromLittleEndian<uint32_t>((uchar*)temp.constData());
if (!switched)
stlink->WriteRegister(reg,val);
ans = "OK";
MakePacket(ans);
}
//stepping
else if(data.startsWith("s"))
{
if (data.count() > 1)
{
//step addr
params_t par = ParseParams(data);
}
else
{
//single step
stlink->CoreSingleStep();
}
ans = "S05";
MakePacket(ans);
}
//thread info
else if(data.startsWith("T"))
{
params_t par = ParseParams(data);
uint32_t id = par[0].toInt(NULL,16);
if (id == THD_MAIN)
ans = "OK";
if (id == THD_HAN)
{
if (stlink->GetMode() == QStLink::Handler)
ans = "OK";
else
ans = "E00";
}
MakePacket(ans);
}
//continue
else if (data.startsWith("c"))
{
params_t pars = ParseParams(data);
QByteArray temp= pars[0];
if (temp.count())
{
//address where to start
}
ans = "+";
soc = client;
stlink->CoreRun();
}
//baud speed
else if (data.startsWith("b"))
{
ans = "+";
}
//kill program
else if (data == "k" || data == "D")
{
ans = "+";
stlink->BreakpointRemoveAll();
stlink->CoreRun();
}
else if (data.startsWith("vFlashErase"))
{
FlashProgram.clear();
ans = "OK";
MakePacket(ans);
segment = 0;
offset = 0;
lastAddress = 0;
}
else if (data.startsWith("vFlashWrite"))
{
uint32_t addr = (data.mid(12,data.indexOf(":",12)-12)).toInt(NULL,16) - FLASH_BASE;
if (!segment++)
{
offset = addr;
}
//fill zeros - program may be shiftet - bootloader and so on
if (addr )
{
int64_t tempo = addr - offset - lastAddress;
while(tempo-- > 0)
{
lastAddress++;
//tempo = addr - lastAddress;
FlashProgram.append('\0');
}
}
QByteArray temp = data.mid(20);
processEscapeChar(temp);
FlashProgram.append(temp);
#if 0
for (int i = 0 ; i < FlashProgram.count(); i++)
{
QFile file("ch.bin");
file.open(QFile::ReadOnly);
QByteArray temp = file.readAll();
if (FlashProgram.at(i) != temp.at(i))
{
asm("nop");
}
}
#endif
lastAddress = FlashProgram.count();
ans = "OK";
MakePacket(ans);
}
else if (data.startsWith("vFlashDone"))
{
QFile file(VeriFile);
if (file.open(QFile::ReadOnly))
{
QByteArray fil = file.readAll();
QByteArray ringFile;
QByteArray ringGdb;
for (int i = 0; i < fil.count(); i++)
{
ringFile.append(fil.at(i));
ringGdb.append(FlashProgram.at(i));
if (ringFile.count() > 10)
ringFile.remove(0,1);
if (ringGdb.count() > 10)
ringGdb.remove(0,1);
}
}
std::cout << "Program size " << FlashProgram.count() << " bytes" << std::flush;
stlink->FlashWrite(offset + FLASH_BASE,FlashProgram, !NotVerify);
segment = 0;
if (ans.count() == 0)
ans = "OK";
MakePacket(ans);
asm("nop");
}
client->write(ans);
}
int main(int argc, char *argv[]) {
static int selret;
unsigned int recLen;
static char* p;
static char filebuffer[FILESIZE];
static char directemp[BUFSIZE];
static char filename[BUFSIZE];
static struct timeval time;
static fd_set readFDS;
static frame frameArray[MAXSEQNUM];
static int LB = 0, RB = 0; // static
static char ackBuffer[100];
static ack recvAck;
static timeStruct timesArray[SWS];
int lastSeqNum = -1;
double t, timeout;
int lastFrameCount = 0;
//check command line args.
if(argc < 6) {
printf("usage : %s <server> <error rate> <random seed> <send_file> <send_log> \n", argv[0]);
exit(1);
}
printf("argv4: %s\n",argv[4]);
if(strcmp(argv[4],"0")){
strcpy(directemp,argv[4]);
p = strtok(directemp,DELM);
while(p != NULL){
strcpy(filename,p);
p= strtok(NULL,DELM);
}
printf("Filename: %s\n",filename);
fp = fopen(argv[4],"r");
if(fp == NULL){
printf("Error opening file: %s\n",strerror(errno));
}
//strcpy(msgbuffer,filebuffer);
}
printf("error rate : %f\n",atof(argv[2]));
printf("%s: sending data to '%s' \n", argv[0], argv[1]);
/* Note: you must initialize the network library first before calling
sendto_(). The arguments are the <errorrate> and <random seed> */
init_net_lib(atof(argv[2]), atoi(argv[3]));
/* get server IP address (input must be IP address, not DNS name) */
//setup socket structs
/***************************************************/
remoteServAddr.sin_family = AF_INET;
remoteServAddr.sin_port = htons(REMOTE_SERVER_PORT);
inet_pton(AF_INET, argv[1], &remoteServAddr.sin_addr);
cliAddr.sin_family = AF_INET;
cliAddr.sin_addr.s_addr = htonl(INADDR_ANY);
cliAddr.sin_port = htons(0);
/*************************************************/
/* socket creation */
if((sock = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
printf("Unable to connect socket!\n");
exit(1);
}
int lastFrameSent = 0;
while(1){
bzero(&ackBuffer, sizeof(ackBuffer));
int moveCount = MoveForward(&LB, &RB, frameArray, MAXSEQNUM);
if(lastFrameSent == 0){
lastFrameSent = SendNextFrames(moveCount, frameArray, MAXSEQNUM, LB, &fp,
&sock,(struct sockaddr *) &remoteServAddr, timesArray, SWS);
}
/*
if((sendto_(sock,sendFrame, strlen(sendFrame),0, (struct sockaddr *) &remoteServAddr,
sizeof(remoteServAddr)))< 0 ){
printf("Error Sending\n");
perror("sendto()");
exit(1);
}*/
gettimeofday(&time,NULL);
t = (time.tv_sec + (time.tv_usec/1000000.0));
timeout = (currentDeadline(timesArray,SWS) + DELAY) - t;
printf("Timeout: %f\n", timeout);
if(timeout < 0){timeout = 0;}
selret = ballinselect(sock,&readFDS,timeout,0);
if ((selret != -1) && (selret != 0)){
if(recvfrom(sock, &ackBuffer, sizeof (ackBuffer), 0,(struct sockaddr *) &recAddr, (socklen_t*) &recLen) < 0){
perror("recvfrom()");
exit(1);
}
printf("Ack Buffer: %s\n",ackBuffer);
makeackstruct(ackBuffer, &recvAck);
if(lastSeqNum != recvAck.seqNum){
frameArray[recvAck.seqNum].ack = 1;
lastSeqNum = recvAck.seqNum;
printf("============================\n");
printf("Received Ack!\n");
printf("Ack SeqNum: %d\n", recvAck.seqNum);
printf("============================\n\n\n");
removefromtimearray(recvAck.seqNum, timesArray,SWS);
}
}
else if (selret == 0) {
printf("timeout\n");
bzero(&filebuffer,sizeof(filebuffer));
int timeoutframe = FindTimeout(timesArray,SWS);
if(frameArray[timeoutframe].lastFrame == 1){
if(frameArray[timeoutframe].ack == 1){return EXIT_SUCCESS;}
if(lastFrameCount == 10){return EXIT_SUCCESS;}
lastFrameCount ++;
}
MakePacket(filebuffer,frameArray[timeoutframe]);
printf("\nSending Timed out Frame: \n\n");
printFrame(frameArray[timeoutframe]);
if((sendto_(sock,filebuffer, strlen(filebuffer),0, (struct sockaddr *) &remoteServAddr,
sizeof(remoteServAddr)))< 0 ){
printf("Error Sending\n");
perror("sendto()");
exit(1);
}
/*gettimeofday(&time,NULL);
double t2= time.tv_sec + (time.tv_usec/1000000.0);
printf("Time Out Time time: %f\n",t2);*/
} else if (selret < 0) {printf("select() failed\n");}
}
fclose(fp);
return EXIT_SUCCESS;
}
// is_enabled is a 0 for no torque or 1 for torque
int DarwinController::Set_Torque_Enable(unsigned char joint_ID, unsigned char is_enabled){
unsigned char Torque_packet[8] = {0, };
MakePacket(Torque_packet, joint_ID, 1, WRITE, TORQUE_ENABLE, &is_enabled);
unsigned char rxpacket[MAXNUM_RXPARAM] = {0, };
return ReadWrite(Torque_packet, rxpacket);
}
int DarwinController::Set_D_Gain(unsigned char joint_ID, unsigned char D_Value){
unsigned char D_packet[8] = {0, };
MakePacket(D_packet, joint_ID, 1, WRITE, D_GAIN, &D_Value); // Pass in pointer b/c it expects a char*
unsigned char rxpacket[MAXNUM_RXPARAM] = {0, };
return ReadWrite(D_packet, rxpacket);
}