From d39107bc98614e7aab867b7aac0bb575494a49b6 Mon Sep 17 00:00:00 2001
From: Tejasvee Panwar <panwar.tejasvee@gmail.com>
Date: Thu, 24 Oct 2019 13:04:59 +0530
Subject: [PATCH 1/2] Added code for new motor driver

---
 RMCS2301_RMCS2004/new_driver/README.md           |  2 ++
 .../new_driver/motor_pwm_control.ino             | 16 ++++++++++++++++
 2 files changed, 18 insertions(+)
 create mode 100644 RMCS2301_RMCS2004/new_driver/README.md
 create mode 100644 RMCS2301_RMCS2004/new_driver/motor_pwm_control.ino

diff --git a/RMCS2301_RMCS2004/new_driver/README.md b/RMCS2301_RMCS2004/new_driver/README.md
new file mode 100644
index 0000000..b5b8779
--- /dev/null
+++ b/RMCS2301_RMCS2004/new_driver/README.md
@@ -0,0 +1,2 @@
+## RMCS 2004 Motor
+Previously used motor driver (RMCS 2301) works erratically. Code for a different motor driver. 
\ No newline at end of file
diff --git a/RMCS2301_RMCS2004/new_driver/motor_pwm_control.ino b/RMCS2301_RMCS2004/new_driver/motor_pwm_control.ino
new file mode 100644
index 0000000..9011625
--- /dev/null
+++ b/RMCS2301_RMCS2004/new_driver/motor_pwm_control.ino
@@ -0,0 +1,16 @@
+
+
+
+void setup() {
+  pinMode(8,OUTPUT);
+  pinMode(9,OUTPUT);
+  digitalWrite(8,LOW);
+  analogWrite(9,128);
+  // put your setup code here, to run once:
+
+}
+
+void loop() {
+
+  
+}

From 7680800c71ab64395d55cb17f775e2e870560fdf Mon Sep 17 00:00:00 2001
From: Tejasvee Panwar <panwar.tejasvee@gmail.com>
Date: Fri, 25 Oct 2019 11:52:32 +0530
Subject: [PATCH 2/2] Added code for encoder reading

---
 .../new_driver/motor_pwm_control.ino          | 38 ++++++++++++++++++-
 1 file changed, 36 insertions(+), 2 deletions(-)

diff --git a/RMCS2301_RMCS2004/new_driver/motor_pwm_control.ino b/RMCS2301_RMCS2004/new_driver/motor_pwm_control.ino
index 9011625..dd9dd83 100644
--- a/RMCS2301_RMCS2004/new_driver/motor_pwm_control.ino
+++ b/RMCS2301_RMCS2004/new_driver/motor_pwm_control.ino
@@ -1,16 +1,50 @@
 
 
 
+#include <ros.h>
+#include <geometry_msgs/Twist.h>
+#include <digitalWriteFast.h>
+#include <std_msgs/Int16.h>
+
+ros::NodeHandle nh;
+int TicksCount=4;
+std_msgs::Int16 ticks;
+int speed=0;
+void messageCb( const geometry_msgs::Twist& toggle_msg){
+
+ speed=int(toggle_msg.linear.x*900);
+
+ }
+ros::Publisher chatter("chatter",&ticks);
+
+ros::Subscriber<geometry_msgs::Twist> sub("cmd_vel", &messageCb );
+
+
 void setup() {
+ Serial.begin(57600);
+  
+  nh.initNode();
+  nh.subscribe(sub);
   pinMode(8,OUTPUT);
   pinMode(9,OUTPUT);
   digitalWrite(8,LOW);
-  analogWrite(9,128);
+  attachInterrupt(digitalPinToInterrupt(2), HandleRightMotorInterruptA, RISING);
+  nh.advertise(chatter);
+  
   // put your setup code here, to run once:
 
 }
+void HandleRightMotorInterruptA()
+{
+  // count the ticks between some interval and calculate the rotation rate in the interval
+  TicksCount++;
+  }
 
 void loop() {
 
-  
+  nh.spinOnce();
+  delay(1);
+  analogWrite(9,speed);
+  ticks.data=TicksCount;
+  chatter.publish(&ticks);
 }