bool PorscheSteeringWheel::homeWheel()
{
bool run;
bool success = true;
unsigned char pdodata[8];
//std::cerr << "Starting node... ";
if (!startNode())
success = false;
//std::cerr << "Enabling operation... ";
if (!enableOp())
success = false;
//std::cerr << "Setting operation mode... ";
if (!setOpMode(6))
success = false;
//std::cerr << "Setting up homing mode... ";
if (!setupHoming(0, 33, 100000, 10))
success = false;
;
//std::cerr << "Starting homing... ";
if (!enableHoming())
success = false;
//std::cerr << "Waiting till homing is finished... ";
run = true;
unsigned long starttime = getTimeInSeconds();
while (run)
{
bus->recvPDO(1, 1, pdodata);
if ((pdodata[1] & 0x14) == 0x14)
run = false;
if ((getTimeInSeconds() - starttime) > 10)
{
run = false;
success = false;
}
std::cerr << "Elapsed time: " << (getTimeInSeconds() - starttime) << std::endl;
}
//std::cerr << "homing finished!" << std::endl;
//std::cerr << "Shutting down... ";
if (!shutdown())
success = false;
//std::cerr << "Stopping node... ";
if (!stopNode())
success = false;
return success;
}
bool PorscheSteeringWheel::stopAngleTorqueMode()
{
bool success = true;
if (!stopLifeGuarding())
success = false;
if (!sendGuardMessage())
success = false;
if (!shutdown())
success = false;
if (!stopNode())
success = false;
return success;
}
void tsClient :: processMsg()
{
int nodeId = 0;
float maxSpeed = 0;
if(inMsg.readByte()==19)
{
//to do
}
char commandType = inMsg.readChar();
switch(commandType)
{
case CMD_SIMSTEP:
PRINTF( INFO, "TS : Command Simstep recieved from NS\n" );
targetTime = inMsg.readDouble() * 1000; //convert to milliseconds
timeInterval = inMsg.readDouble() * 1000; //convert to millis
PRINTF( INFO, "TS : Set Automatic Periodic update after : %f time\n",timeInterval );
if(inMsg.readChar()==POSITION_2D) //postion verification
{
//to do
}
periodicTimer = true;
makeSendMsg(NS_CONN);
break;
case CMD_ACK:
PRINTF( INFO, "TS : Command Ack Recieved\n" );
break;
case CMD_HBW:
PRINTF( INFO, "TS : Warning Message Recieved\n" );
break;
case CMD_SETMAXSPEED:
nodeId = inMsg.readInt();
maxSpeed = inMsg.readFloat();
nodes[indexMap[nodeId]].maxSpeed = maxSpeed;
if(nodes[indexMap[nodeId]].speed > maxSpeed)
nodes[indexMap[nodeId]].speed = maxSpeed;
PRINTF( INFO, "TS : Set Max Speed Cmd\n" );
break;
case CMD_STOP:
stopNode();
break;
case CMD_CHANGELANE:
break;
case CMD_CHANGEROUTE:
break;
case CMD_CHANGETARGET:
break;
/*
Other Traci Commands Handler Here, stop Node etc
*/
case CMD_DS_CONN_SETUP:
addDSNode();
PRINTF( INFO, "TS : DS Connection setup notify\n" );
break;
case CMD_DS_CONN_REM:
deleteDSNode();
tv.tv_sec = 0;
tvptr = NULL;
periodicTimer = false;
firstDSNodeUpdate = true;
PRINTF( INFO, "TS : DS Connection Remove notify\n" );
break;
case CMD_DS_UPDATE:
PRINTF( INFO, "TS : Update from IMS Recived\n");
UpdateDsNodePos();
if(firstDSNodeUpdate) {
dsTimeInterval = P_TIMER;
dsTargetTime += dsTimeInterval;
firstDSNodeUpdate = false;
periodicTimer = true;
}
break;
case CMD_NS_CONN_REM: //TS closes the connection, reset present and target time and stop sending update
//after every disconnection start simulation from scratch, node pos back to zero
presentTime = targetTime = 0;
tv.tv_sec = 0;
tvptr = NULL;
periodicTimer = false;
timeInterval = 0.0;
PRINTF( INFO, "TS : NS is dead, stop sending update and reset timer\n" );
default:
PRINTF( ERROR, "TS : No Handler Written for this Event..!!!!\n" );
}
}