int main()
{
// SET single LED port
DDRC |= _BV(LED_PIN);
PORTC ^= _BV(LED_PIN);
// 1s pull up of LED Strip pins */
apa102_DDRREG &= ~_BV(apa102_data);
apa102_DDRREG &= ~_BV(apa102_clk);
apa102_PORTREG |= _BV(apa102_data);
apa102_PORTREG |= _BV(apa102_clk);
_delay_ms(900);
// Disable pull ups
apa102_PORTREG &= ~_BV(apa102_data);
apa102_PORTREG &= ~_BV(apa102_clk);
apa102_DDRREG |= _BV(apa102_data);
apa102_DDRREG |= _BV(apa102_clk);
_delay_ms(100);
PORTC ^= _BV(LED_PIN);
// Clear LEDs in the strip
clear_all_leds();
// Init ROS
nh.initNode();
nh.subscribe(set_sub);
nh.subscribe(set_led_sub);
ack_led();
// Wait for Server side to start
while (!nh.connected())
{
nh.spinOnce();
// LUFA functions that need to be called frequently to keep USB alive
CDC_Device_USBTask(&Atmega32u4Hardware::VirtualSerial_CDC_Interface);
USB_USBTask();
_delay_ms(10);
}
ack_led();
// Publish some debug information
snprintf(log_str, MAX_MSG_SIZE, "V:%s", GIT_VERSION);
nh.loginfo(log_str);
snprintf(log_str, MAX_MSG_SIZE, "FM:%d", get_free_ram());
nh.loginfo(log_str);
while(1)
{
nh.spinOnce();
// LUFA functions that need to be called frequently to keep USB alive
CDC_Device_USBTask(&Atmega32u4Hardware::VirtualSerial_CDC_Interface);
USB_USBTask();
}
return 0;
}
void setup() {
nh.initNode();
last_time = nh.now();
current_time = nh.now();
//Serial.begin(38400);
//#####delay(1000);
rlog = new RosLogger(nh);
//#####quadratureEncoder = new QuadratureEncoder();
// lineSensor = new LineSensor();
// lineSensor->calibrate();
nh.advertise(odom_pub);
motor = new Motor(nh);
/*
Motor::Command c;
c.direction = Motor::STOP; motor->enqueue(c);
c.direction = Motor::BACKWARD; motor->enqueue(c);
c.direction = Motor::FORWARD; motor->enqueue(c);
c.direction = Motor::STOP; motor->enqueue(c);
c.direction = Motor::RIGHT_TURN; motor->enqueue(c);
c.direction = Motor::STOP; motor->enqueue(c);
c.direction = Motor::BACKWARD; motor->enqueue(c);
c.direction = Motor::STOP; motor->enqueue(c);
c.direction = Motor::LEFT_TURN; motor->enqueue(c);
c.direction = Motor::STOP; motor->enqueue(c);
*/
nh.subscribe(Motor::sub);
while (!nh.connected()) { nh.spinOnce(); }
rlog->info("START UP...");
}
int main(int argc, char* argv[])
{
// These following lines are used to Make sure that command lline args are correct
if(argc<5) // if number of args are less than 5
{
cerr << "Usage: " << argv[0] << " <socket> <left_motor_port> <right_motor_port> <sensor_port> <hz>" << endl;
return 1;
}
int milliseconds = 100;
if(argc==6)
milliseconds = 1000/atoi(argv[5]);
// cout<<"milliseconds"<<milliseconds;
string left_motor_port(argv[2]);
string right_motor_port(argv[3]);
string sensor_port(argv[4]);
// if(left_motor_port<1||left_motor_port>4||right_motor_port<1||right_motor_port>4||left_motor_port==right_motor_port)
// {
// cerr << "Invalid motor port numbers. Must be 1, 2, 3 or 4 and distinct." << endl;
// return 1;
// }
// TODO: Check if both are of same type
left_motor = motor(left_motor_port);
right_motor = motor(right_motor_port);
s = sensor(sensor_port);
if(s.type()!="ev3-uart-30") // sensor object s will not be used hereafter
{
cerr << "Invalid sensor type. Must be EV3 ultrasonic. Given sensor is of type " << s.type() << endl;
return 1;
}
//---------------------------------------------------------------------------------------------------------------------------------------------
// TODO: Check if both were initialised
left_motor.reset();
left_motor.set_position(0);
left_motor.set_run_mode("time");// changed from forever mode to time
left_motor.set_stop_mode("brake");
left_motor.set_regulation_mode("on");
right_motor.reset();
right_motor.set_position(0);
right_motor.set_run_mode("time");
right_motor.set_stop_mode("brake");
right_motor.set_regulation_mode("on");
nav_msgs::Odometry odom_msg;// msg object for the publisher
ros::Publisher odom_pub("/robot3/odom"/*topic name*/, &odom_msg);
nh.advertise(odom_pub); // advertises that odom_pub is the publisher name
tf::TransformBroadcaster odom_broadcaster; //broadcaster msg for the odometry
tf::TransformBroadcaster scan_broadcaster; // broadcaster for the ultrasonic sensor
nh.subscribe(pose_sub);
ros::Time current_time, last_time;
current_time = nh.now();
last_time = nh.now();
nh.initNode(argv[1]);
odom_broadcaster.init(nh);
//nh.advertiseService(server1);
nh.advertiseService(server2);
while(!nh.connected()) {nh.spinOnce();}
// JUST TO KNOW IF THE NODE IS ALIVE
cout<<"1. Gostraight service\n2.Trace an arc service"<<endl;
while(1) {
// check for incoming messages
current_time = nh.now();
geometry_msgs::Quaternion odom_quat = tf::createQuaternionFromYaw(t); // odom_quat stores Quaternion cretaed from yaw
// cout<<" Calculated quat: "<<endl;
//first, we'll publish the transform over tf
geometry_msgs::TransformStamped odom_trans;//message object
odom_trans.header.stamp = current_time;
odom_trans.header.frame_id = map_name;//map_name is just given as map
const char base_link_name[18] = "/robot3/base_link";
odom_trans.child_frame_id = base_link_name;
// cout<<" constructed header"<<endl;
// loading the message object with data
odom_trans.transform.translation.x = x;
odom_trans.transform.translation.y = y;
odom_trans.transform.translation.z = 0.0;// as we are dealing with xy plane
odom_trans.transform.rotation = odom_quat;// this is quaternion created from yaw angle t
// cout<<" Constructed full message"<<endl;
//send the transform
odom_broadcaster.sendTransform(odom_trans);
//next, we'll publish the odometry message over ROS
odom_msg.header.stamp = current_time;
odom_msg.header.frame_id = map_name;
//set the position
odom_msg.pose.pose.position.x = x;
odom_msg.pose.pose.position.y = y;
odom_msg.pose.pose.position.z = 0.0;
odom_msg.pose.pose.orientation = odom_quat;
//set the velocity
odom_msg.child_frame_id = base_link_name;
odom_msg.twist.twist.linear.x = vx;
odom_msg.twist.twist.linear.y = 0;
odom_msg.twist.twist.angular.z = wt;
// cout<<" sizeof "<<sizeof(odom_msg)<<endl;
//publish the message
odom_pub.publish(&odom_msg);
cout<<"Services are being advertised. Waiting for request"<<endl;
nh.spinOnce();
sleep(1);
}
return 0;
}