int main(int argc, char** argv)
{
ros::init(argc, argv, "Wander");
ros::NodeHandle nh;
ros::Rate loop_rate(LOOP_RATE);
//Publisher
motorPub = nh.advertise<robot::motor>("motor_command", 1000);
servoPub = nh.advertise<std_msgs::UInt8>("servo_command", 1);
loop_rate.sleep();
//Subscriber
ros::Subscriber maestroSub = nh.subscribe<robot::sensors>("sensor_data", 2, processSensors);
ros::Subscriber xmegaSub = nh.subscribe<std_msgs::UInt8>("xmega_feedback", 1, xmegaFeedback);
//initialize servos
std_msgs::UInt8 msg;
msg.data = SERVO_INIT;
servoPub.publish(msg);
while(ros::ok())
{
//In wander state, robot moves forward with a duty cycle of 70
if (state == WANDER)
{
if ((midIR > MID_VN) || (leftIR > LEFT_VN) || (rightIR > RIGHT_VN))
{
avoid_obstacle();
}
sendMotorCommand(GO_FORWARD, 70);
ros::spinOnce();
}
loop_rate.sleep();
}
}
char go_prosto() {
char result = 0;
char step_count = 0;
if (val == 2) {
go_prosto_count++;
}
for (; step_count < 38; step_count++) {
ADC_StartDoubleChannelConversion(ADC_CHANNEL_7, ADC_CHANNEL_5,
onDistanceDataRdy);
while (!ADC_IsDataReady())
;
OAA_OUTPUT oa_result = avoid_obstacles(level_mask);
if (oa_result.gear_left == oa_result.gear_right) {
PWM_Set(0, oa_result.speed_left);
Kierunek(RIGHT_ENGINES, FORWARD_GEAR);
Kierunek(LEFT_ENGINES, FORWARD_GEAR);
waitms(100);
}
result = avoid_obstacle();
}
return result;
}