task main()
{
initializeRobot(ML, MR, HandL, HandR, ScissorL1, ScissorR1);
waitForStart();
while (nMotorEncoder[ML] < 4000 && nMotorEncoder[MR] < 4000) { // Forward a bit
motor[ML] = 75;
motor[MR] = 75;
}
resetMotors(ML, MR);
while (nMotorEncoder[ML] < 3333 && nMotorEncoder[MR] > -3333) { // Turn right 90 degrees
motor[ML] = 75;
motor[MR] = -75;
}
resetMotors(ML, MR);
while (nMotorEncoder[ML] < 4000 && nMotorEncoder[MR] < 4000) {
motor[ML] = 75;
motor[MR] = 75;
}
resetMotors(ML, MR);
while (nMotorEncoder[ML] < 3333 && nMotorEncoder[MR] > -3333) { // Turn right 90 degrees
motor[ML] = 75;
motor[MR] = -75;
}
resetMotors(ML, MR);
while (nMotorEncoder[ML] < 4000 && nMotorEncoder[MR] < 4000) {
motor[ML] = 75;
motor[MR] = 75;
}
}
//*MAIN*//
int main(int argc, char * argv[])
{
bool active = true; //flag that controls main loop
int value; //value of received message
char messageType;
unsigned int serialPort;
char * serialAddr = "/dev/ser1";
TCP tcpConnection;
unsigned int clientSocket;
//Initialization
//Parse the arguments
switch (argc)
{
case 2:
serialAddr = argv[1];
break;
}
if ( argc >= 2 )
{
if ( initSerial(serialPort, serialAddr) == false )
{
std::cout << "Serial port initialization failure.\n";
return 0;
}
if ( initMotors() == false )
{
std::cout << "Motor initialization failure.\n";
return 0;
}
}
if ( initTCP(tcpConnection, clientSocket) == false )
{
std::cout << "TCPIP initialization failure.\n";
return 0;
}
while (active)
{
tcpConnection.receiveData( clientSocket,
(char *) &messageType,
sizeof( messageType ));
if(tcpConnection.receiveData( clientSocket,
(char *) &value,
sizeof( value )) == DATA_ERROR)
{
active = false;
std::cout << "Client Disconnected!" << std::endl;
}
#ifdef DEBUG
debugprint(messageType, value);
#endif
if(messageType == 'X')
active = false;
else if (active == true)
{
#ifdef ENABLE_STEERING
if (messageType == 'S')
Move_Motor_Abs(value);
else
#endif
motorControl(serialPort, messageType, value);
//watch out for this!
}
}
std::cout << "Resetting motors..." << std::endl;
resetMotors(serialPort);
#ifdef ENABLE_STEERING
Close_Maxon_Motor_Driver();
#endif
tcpConnection.closeSocket(clientSocket);
std::cout << "Terminating..." << std::endl;
return 0;
}
