int main() {
unsigned long last_pub;
/* set up interrupt handling */
// set up timer interrupts
// fast PWM mode; interrupt and reset when counter equals OCR0A
// prescalar 64
TCCR0A = (1 << WGM01 | 1 << WGM00);
TCCR0B = (1 << WGM02 | 1 << CS01 | 1 << CS00);
// interrupt on "overflow" (counter match)
TIMSK0 = (1 << TOIE0);
OCR0A = 249; // 250 counts per tick
nh.initNode();
nh.advertise(pub);
nh.subscribe(sub);
last_pub = nh.now().toNsec();
while(1) {
nh.spinOnce();
// do our best to publish once per second
if( nh.now().toNsec() - last_pub > 1000000000ull ) {
pub.publish(&msg);
last_pub += 1000000000ull;
}
}
}
int main()
{
int error = 0;
t.start();
nh.initNode();
nh.advertise(pub_temp);
nh.advertise(pub_humidity);
long publisher_timer = 0;
temp_msg.header.frame_id = "/base_link";
humidity_msg.header.frame_id = "/base_link";
while (1)
{
if (t.read_ms() > publisher_timer)
{
error = sensor.readData();
if (0 == error)
{
temp_msg.temperature = sensor.ReadTemperature(CELCIUS);
temp_msg.header.stamp = nh.now();
pub_temp.publish(&temp_msg);
humidity_msg.relative_humidity = sensor.ReadHumidity();
humidity_msg.header.stamp = nh.now();
pub_humidity.publish(&humidity_msg);
}
publisher_timer = t.read_ms() + 1000;
}
nh.spinOnce();
}
}
void setup() {
// put your setup code here, to run once:
StepMotor.setSpeed(150);
pinMode(Button, INPUT_PULLUP);
pinMode(Sensor_Down, INPUT_PULLUP);
pinMode(Sensor_Up, INPUT_PULLUP);
pinMode(EnA, OUTPUT);
pinMode(EnB, OUTPUT);
nh.initNode();
nh.subscribe(sub);
nh.advertise(answer_pub);
nh.advertise(switch_pub);
if(digitalRead(Button)){
deadman_activated = true;
deadman_switch_.data = deadman_activated;
deadman_switch_.header.stamp = nh.now();
switch_pub.publish(&deadman_switch_);
}
else{
deadman_activated = false;
deadman_switch_.data = deadman_activated;
deadman_switch_.header.stamp = nh.now();
switch_pub.publish(&deadman_switch_);
}
down();
}
void loop() {
if(digitalRead(Button) && deadman_activated == false){
deadman_activated = true;
deadman_switch_.data = deadman_activated;
deadman_switch_.header.stamp = nh.now();
switch_pub.publish(&deadman_switch_);
}
else if(!digitalRead(Button) && deadman_activated == true){
deadman_activated = false;
deadman_switch_.data = deadman_activated;
deadman_switch_.header.stamp = nh.now();
switch_pub.publish(&deadman_switch_);
}
nh.spinOnce();
delay(50);
}
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...");
}
void loop()
{
//publish the adc value every 50 milliseconds
//since it takes that long for the sensor to stablize
if ( millis() >= range_time ){
int r =0;
range_msg.range = getRange_Ultrasound(5);
range_msg.header.stamp = nh.now();
pub_range.publish(&range_msg);
range_time = millis() + 50;
}
nh.spinOnce();
}
void down(){
digitalWrite(EnA, HIGH);
digitalWrite(EnB, HIGH);
StepMotor.setSpeed(150);
if(Sensor_Down!=1){
while(digitalRead(Sensor_Down)!=1){
StepMotor.step(-stepsPerRevolution/4);
}
}
digitalWrite(EnA, LOW);
digitalWrite(EnB, LOW);
state_ = 2;
answer_.data = state_;
answer_.header.stamp = nh.now();
answer_pub.publish(&answer_);
}
void loopUltra()
{
// establish variables for duration of the ping,
// and the distance result in inches and centimeters:
long duration, inches, cm;
if (intervalStarted) {
// The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
// Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
pinMode(sonarPin, OUTPUT);
digitalWrite(sonarPin, LOW);
delayMicroseconds(2);
digitalWrite(sonarPin, HIGH);
delayMicroseconds(15);
digitalWrite(sonarPin, LOW);
delayMicroseconds(20);
// The same pin is used to read the signal from the PING))): a HIGH
// pulse whose duration is the time (in microseconds) from the sending
// of the ping to the reception of its echo off of an object.
pinMode(sonarPin, INPUT);
duration = pulseIn(sonarPin, HIGH);
// convert the time into a distance
cm = microsecondsToCentimeters(duration);
// put data into message
range_msg_sonar.radiation_type = sensor_msgs::Range::ULTRASOUND;
range_msg_sonar.header.frame_id = "/ultraSonic"; // frame
range_msg_sonar.header.stamp = nh.now(); // time
range_msg_sonar.field_of_view = 60.0 / 360 * 3.14; // [rad]
range_msg_sonar.min_range = 0.03; // [m]
range_msg_sonar.max_range = 4.0; // [m]
range_msg_sonar.range = cm / 100.0; // [m]
// publish
pub_range.publish(&range_msg_sonar);
}
}
void loop() {
nh.spinOnce();
current_time = nh.now();
vx = motor->leftVelocity();
vy = motor->rightVelocity();
vth = motor->zVelocity();
double dt = (current_time.toNsec() - last_time.toNsec()) / (1000000000.0);
double delta_x = (vx * cos(th) - vy * sin(th)) * dt;
double delta_y = (vx * sin(th) + vy * cos(th)) * dt;
double delta_th = vth * dt;
x += delta_x;
y += delta_y;
th = delta_th;
geometry_msgs::Quaternion odom_quat = createQuaternionMsgFromYaw(th);
geometry_msgs::TransformStamped odom_trans;
odom_trans.header.stamp = current_time;
odom_trans.header.frame_id = "odom";
odom_trans.child_frame_id = "base_link";
odom_trans.transform.translation.x = x;
odom_trans.transform.translation.y = y;
odom_trans.transform.translation.z = 0.0;
odom_trans.transform.rotation = odom_quat;
//odom_broadcaster.sendTransform(odom_trans);
nav_msgs::Odometry odom;
static unsigned long odomSeq = 0;
odom.header.seq = odomSeq++;
odom.header.stamp = current_time;
odom.header.frame_id = "odom";
//set the position
odom.pose.pose.position.x = x;
odom.pose.pose.position.y = y;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation.x = odom_quat.x;
odom.pose.pose.orientation.y = odom_quat.y;
odom.pose.pose.orientation.z = odom_quat.z;
odom.pose.pose.orientation.w = odom_quat.w;
//set the velocity
odom.child_frame_id = "base_link";
odom.twist.twist.linear.x = vx;
odom.twist.twist.linear.y = vy;
odom.twist.twist.linear.z = 0.0;
odom.twist.twist.angular.x = vth;
odom.twist.twist.angular.y = vth;
odom.twist.twist.angular.z = vth;
for (int i = 0; i < 36; i++) {
odom.pose.covariance[i] = 0.0;
odom.twist.covariance[i] = 0.0;
}
//publish the message
odom_pub.publish(&odom);
// lineSensor->read();
// const LineSensor::TSensorArray& values = lineSensor->sensorValues();
// sprintf(buffer, "Position: %d, values: L> %d, %d, %d, %d, %d, %d, %d, %d <R",
// lineSensor->position(),
// values[0],
// values[1],
// values[2],
// values[3],
// values[4],
// values[5],
// values[6],
// values[7]
// );
// rlog->info("Number: %d, quad: %d", loopCounter++, QuadratureEncoder::Counter());
// rlog->info(buffer);
//rlog->info("Queue len: %d", motor->queueLength());
//quadratureEncoder->info();
motor->run();
last_time = current_time;
//delay(10);
}
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;
}
void darthVader(){
digitalWrite(EnA, HIGH);
digitalWrite(EnB, HIGH);
StepMotor.setSpeed(150);
StepMotor.step(-stepsPerRevolution);
delay(500);
//Empirial March
StepMotor.setSpeed(20);
StepMotor.step(35);
delay(60);
StepMotor.step(-35);
delay(60);
StepMotor.step(35);
delay(60);
StepMotor.setSpeed(15);
StepMotor.step(-20);
delay(50);
StepMotor.setSpeed(24);
StepMotor.step(14);
StepMotor.setSpeed(20);
StepMotor.step(-35);
delay(30);
StepMotor.setSpeed(15);
StepMotor.step(20);
delay(50);
StepMotor.setSpeed(24);
StepMotor.step(-14);
StepMotor.setSpeed(20);
StepMotor.step(65);
delay(100);
StepMotor.setSpeed(30);
StepMotor.step(-50);
delay(60);
StepMotor.step(50);
delay(60);
StepMotor.step(-50);
delay(60);
StepMotor.setSpeed(32);
StepMotor.step(40);
delay(50);
StepMotor.setSpeed(24);
StepMotor.step(14);
StepMotor.setSpeed(19);
StepMotor.step(-30);
delay(60);
StepMotor.setSpeed(15);
StepMotor.step(20);
delay(50);
StepMotor.setSpeed(24);
StepMotor.step(-14);
StepMotor.setSpeed(20);
StepMotor.step(70);
digitalWrite(EnA, LOW);
digitalWrite(EnB, LOW);
state_ = 1;
answer_.data = state_;
answer_.header.stamp = nh.now();
answer_pub.publish(&answer_);
}