 /*
  * Squad Collaborative Robotics
  * Prototype Embedded Collector Bot Code
+   Arduino Mega 2560 R3
  */
 
 
 #include <ros.h>
 #include <std_msgs/Int32.h>
+#include <std_msgs/Float64.h>
 #include <Event.h>
 #include <Timer.h>
 #include <Encoder.h>
-//#include <PololuQik.h>
-//#include <SoftwareSerial.h>
+#include <PololuQik.h>
+#include <SoftwareSerial.h>
+#include <scr_proto/DiffCommand.h>
 
 // Encoder stuff
 #define LEFT_ENCODER_A_PIN 2

  L_EncoderVelocity = 1000.0*(float)(L_encoderValue-L_LastEncoderValue)/(float)ENCODER_TIME_DELAY_MS;
  R_EncoderVelocity = 1000.0*(float)(R_encoderValue-L_LastEncoderValue)/(float)ENCODER_TIME_DELAY_MS;

  // Convert to radians/sec
  L_EncoderVelocity /= (double)TICKS_PER_REVOLUTION;
  R_EncoderVelocity /= (double)TICKS_PER_REVOLUTION;
  
  // Set last known encoder ticks
  L_LastEncoderValue = L_encoderValue;
  R_LastEncoderValue = R_encoderValue;
}

ros::Subscriber<std_msgs::Int32> leftMotorSub("left_motor", leftMotorCallback);
ros::Subscriber<std_msgs::Int32> rightMotorSub("right_motor", rightMotorCallback);


// Rx, Tx, Reset Pins
//PololuQik2s12v10 pmd(2, 3, 4);

void setup()
{

  nh.advertise(right_encoder_pub);

  // Motor Driver Init
  pmd.init();
}

void loop()
{
  // Get Freshest Values
  updateEncoderReading();

  // Populate messages
  l_enc_msg.data = L_LastEncoderValue;
  r_enc_msg.data = R_LastEncoderValue;
  l_wheel_msg.data = L_WheelVelocity;
  r_wheel_msg.data = R_WheelVelocity;

  // Publish Data
  left_encoder_pub.publish( &l_enc_msg );
  right_encoder_pub.publish( &r_enc_msg );
  left_wheel_pub.publish( &l_wheel_msg );
  right_wheel_pub.publish( &r_wheel_msg );
  
  // Handle Callbacks
  nh.spinOnce();
  
  // Write Speeds to motor driver
  pmd.setSpeeds(lm_cmd, rm_cmd);
  
  delay(10);
}

