include <ros.h>

include <rospy_tutorials floats.h="">

include <std_msgs empty.h="">

#include <ros.h>
#include <rospy_tutorials/Floats.h>
#include <std_msgs/Empty.h>

//#include "freeram.h"
//#include "mpu.h"
//#include "I2Cdev.h"
//#include "Wire.h"
 
"Wire.h"


//PORTS ASSIGNING
const int motor11       = 8;
const int motor12       = 7;
const int motor21       = 5;
const int motor22       = 4;
const int motorcontrol1 = 6;
const int motorcontrol2 = 11;
 
11;

//MOTOR CONTROL VARIABLES
int motorflag = 0;
float motor1output = 0;
float motor2output = 0;
 
0;

ros::NodeHandle  nh;
 
nh;



float aux[] = {9, 9, 9, 9};
rospy_tutorials::Floats msg;
 
msg;

ros::Publisher IMUdata("IMUdata", &msg);
 
int ret;
 
&msg);

int ret;

void receiveMotorControl(const rospy_tutorials::Floats& controlarray){
  motor1output = controlarray.data[0];
  motor2output = controlarray.data[1];
  motorflag = 1;
  nh.loginfo("Got a callback");
}
ros::Subscriber<rospy_tutorials::floats> ros::Subscriber<rospy_tutorials::Floats> sub("motorControl", &receiveMotorControl);
 
&receiveMotorControl);

void setup()
{ 
  nh.getHardware()->setBaud(57600);
  nh.initNode();
  nh.advertise(IMUdata);
  nh.subscribe(sub);
 
nh.subscribe(sub);

  //Wire.begin();
  //Fastwire::setup(400,0);
 
//Fastwire::setup(400,0);

  //ret = mympu_open(200);
  msg.data_length = 4;
 
4;

  //PORT SETUP
  pinMode(motor11, OUTPUT);
  pinMode(motor12, OUTPUT);
  pinMode(motor21, OUTPUT);
  pinMode(motor22, OUTPUT);
   while (!nh.connected()) 
  {
    nh.spinOnce();
  }
  nh.loginfo("Connected to microcontroller.");
 
}
 
microcontroller.");

}

void loop()
{ 
//  if(motorflag == 1){
//    motorControl();
//  }
 
}

  //ret = mympu_update();
 
mympu_update();

  //if(ret == 0){
  //    aux[0] = mympu.ypr[0];
  //    aux[1] = mympu.ypr[1];
  //    aux[2] = mympu.ypr[2];
  //}
  //aux[3] = ret;
 
ret;

  //msg.data = aux;
 
aux;

  IMUdata.publish( &msg );
  nh.spinOnce();
  delay(1000);
}
 
}

void motorControl(){
  if(motorflag){
    if(motor1output >= 0){
      digitalWrite(motor11, HIGH);
      digitalWrite(motor12, LOW);
      analogWrite(motorcontrol1, motor1output);
    }
    else{
      digitalWrite(motor11, LOW);
      digitalWrite(motor12, HIGH);
      analogWrite(motorcontrol1, abs(motor1output));
    }
    if(motor2output >= 0){
      digitalWrite(motor21, HIGH);
      digitalWrite(motor22, LOW);
      analogWrite(motorcontrol2, motor2output);
    }
    else{
      digitalWrite(motor21, LOW);
      digitalWrite(motor22, HIGH);
      analogWrite(motorcontrol2, abs(motor2output));
    }
    motorflag = 0;
  }
}
```