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();
}
}
//Description: avoids an obstacle
//Calls: stop(), sendMotorCommand(), avoid_obstacle()
int avoid_obstacle()
{
if (midIR > MID_VN)
{
ROS_WARN("Object in front");
sendMotorCommand(GO_BACKWARD, 70);
while(midIR > MID_FAR) { ros::spinOnce(); }
(leftIR > rightIR) ? sendMotorCommand(PIVOT_RIGHT, 70) : sendMotorCommand(PIVOT_LEFT, 70) ;
while(midIR > MID_VF){ ros::spinOnce(); }
} else if (leftIR > LEFT_VN) {
ROS_WARN("Object to left");
sendMotorCommand(PIVOT_RIGHT, 70);
while (leftIR > LEFT_FAR) { ros::spinOnce(); }
} else if (rightIR > RIGHT_VN) {
ROS_WARN("Object to right");
sendMotorCommand(PIVOT_LEFT, 70);
while (rightIR > RIGHT_FAR) { ros::spinOnce(); }
}
}
// Test the motor driver
void testMotorDriver(void)
{
sendMotorCommand(MOTOR_CMD_DRIVE_FORWARD_MOTOR_1, 0);
sendMotorCommand(MOTOR_CMD_DRIVE_FORWARD_MOTOR_2, 0);
while (1)
{
WDT_Reset();
int i;
for (i = 0; i < 20; i++)
{
sendMotorCommand(MOTOR_CMD_DRIVE_FORWARD_MOTOR_1, 60);
_delay_ms(100);
WDT_Reset();
}
for (i = 0; i < 20; i++)
{
sendMotorCommand(MOTOR_CMD_DRIVE_FORWARD_MOTOR_1, 0);
_delay_ms(100);
WDT_Reset();
}
for (i = 0; i <= 60; i++)
{
WDT_Reset();
sendMotorCommand(MOTOR_CMD_DRIVE_FORWARD_MOTOR_1, i);
sendMotorCommand(MOTOR_CMD_DRIVE_FORWARD_MOTOR_2, i);
_delay_ms(20);
}
WDT_Reset();
_delay_ms(100);
for (; i >= 0; i--)
{
WDT_Reset();
sendMotorCommand(MOTOR_CMD_DRIVE_FORWARD_MOTOR_1, i);
sendMotorCommand(MOTOR_CMD_DRIVE_FORWARD_MOTOR_2, i);
_delay_ms(20);
}
WDT_Reset();
_delay_ms(450);
WDT_Reset();
_delay_ms(450);
WDT_Reset();
_delay_ms(450);
WDT_Reset();
_delay_ms(450);
}
}
void update(uint sim_time, uint none)
{
if(sim_time % 2 == 0) {
float rad = 2.0*3.1415*(sim_time % 360)/360.0;
float s = sin(rad);
int coord = (int)(500*(s) + 2000);
sendMotorCommand(2000, coord);
//io_printf(IO_BUF,"coord is %d\n", coord);
}
}
void testJoystickDriveMotors(void)
{
//range = 1150
//zero = 2720
int16_t speed;
int16_t dir;
while(1) {
WDT_Reset();
speed = getWiredPropJoySpeed();
dir = getWiredPropJoyDirection();
speed = (speed - 2715) / 4;
dir = (dir - 2725) / 4;
//printf("Speed: %d Dir: %d\n", speed, dir);
if (speed >= 0) {
if (speed > 127)
speed = 127;
sendMotorCommand(MOTOR_CMD_DRIVE_FORWARD_MIXED_MODE, speed);
} else {
speed = -speed;
if (speed > 127)
speed = 127;
sendMotorCommand(MOTOR_CMD_DRIVE_BACKWARDS_MIXED_MODE, speed);
}
if (dir >= 0) {
if (dir > 127)
dir = 127;
sendMotorCommand(MOTOR_CMD_TURN_RIGHT_MIXED_MODE, dir);
} else {
dir = -dir;
if (dir > 127)
dir = 127;
sendMotorCommand(MOTOR_CMD_TURN_LEFT_MIXED_MODE, dir);
}
}
}
