diff --git a/Robotics/Drivers/Hermes/Hermes.C b/Robotics/Drivers/Hermes/Hermes.C index 47a62c5..b52af7e 100644 --- a/Robotics/Drivers/Hermes/Hermes.C +++ b/Robotics/Drivers/Hermes/Hermes.C @@ -115,33 +115,30 @@ void HermesModule::run() unsigned char dataIn = itsSerialPort->ReadByte(); if(dataIn == SEN_COMPASS) { - NRT_INFO("Compass: " << dataIn); + //NRT_INFO("Compass: " << dataIn); compassPacket packet; for(int i=0; iReadByte(); } - NRT_INFO("Data: " << packet.heading); + //NRT_INFO("Data: " << packet.heading); Message::unique_ptr msg(new Message); msg->value = packet.heading; post(msg); } else if(dataIn == SEN_GYRO) { - NRT_INFO("Gyro: " << dataIn); + //NRT_INFO("Gyro: " << dataIn); gyroPacket packet; for(int i=0; iReadByte(); } - NRT_INFO("Data: X("<::unique_ptr msg(new Message); - msg->value = packet.xyz[2]; + msg->value = (NRT_D_PI/180.0) * packet.xyz[2]; post(msg); } else NRT_INFO("Unrecognized: " << dataIn); } - - NRT_INFO("I'm Awesome"); - sleep(1); } }