Added ros example and fixed overshooting bug

PositionControl.h

@@ -16,60 +16,57 @@ class PositionControl
   uint8_t encB;
   bool polarity_reverse = false;
 
-  //Motor Related Parameters
-  
+  // Motor Related Parameters
+
   float motorRadius = 1; // in metres
-  float maxSpeed = 2.0; // Rotation Per Second
+  float maxSpeed = 2.0;  // Rotation Per Second
 
   // PID Control Related Parameter
-  double outputMin  = -50;
+  double outputMin = -50;
   double outputMax = 50;
-  float kP  = 0.1;
-  float kI  = 0.03;
-  float kD  = 0.02;
-
+  float kP = 0.1;
+  float kI = 0.03;
+  float kD = 0.02;
 
-public: 
-  //Velocity control related parameters
+public:
+  // Velocity control related parameters
   double currentPosition;
   long CPR = 36124;
   double setPoint = 0;
-  double outputPWM= 0;
+  double outputPWM = 0;
   double tolerance = 0.1;
 
   // double * position;
   AutoPID pid{&currentPosition, &setPoint, &outputPWM, outputMin, outputMax, kP, kI, kD};
-  Encoder * encoder;
-  
-  void  initialSetup()
+  Encoder *encoder;
+
+  void initialSetup()
   {
-  // Setting the pin configurations
-  pinMode(dirPin, OUTPUT);
-  pinMode(pwmPin, OUTPUT);
-  digitalWrite(dirPin , LOW);
-  analogWrite(pwmPin , 0);
-  delay(2000);
-  //variables for velocity calculation
-  setPoint  = 0.0;
-  currentPosition = 0;
-  outputPWM  = 0;
+    // Setting the pin configurations
+    pinMode(dirPin, OUTPUT);
+    pinMode(pwmPin, OUTPUT);
+    digitalWrite(dirPin, LOW);
+    analogWrite(pwmPin, 0);
+    delay(2000);
+    // variables for velocity calculation
+    setPoint = 0.0;
+    currentPosition = 0;
+    outputPWM = 0;
   }
 
   PositionControl()
   {
     // params:  direction pin , pwm pin , encoder A , encoder B
-
   }
 
-  void setPin(uint8_t pinDir , uint8_t pinPWM ,uint8_t aEnc , uint8_t bEnc )
+  void setPin(uint8_t pinDir, uint8_t pinPWM, uint8_t aEnc, uint8_t bEnc)
   {
-  dirPin = pinDir;
-  pwmPin = pinPWM;
-  encA  = aEnc;
-  encB = bEnc;
-  encoder  =  new Encoder(encA , encB);
-  initialSetup();
-
+    dirPin = pinDir;
+    pwmPin = pinPWM;
+    encA = aEnc;
+    encB = bEnc;
+    encoder = new Encoder(encA, encB);
+    initialSetup();
   }
 
   ~PositionControl()
@@ -90,31 +87,31 @@ class PositionControl
 
   void setMotorPWM(int PWM)
   {
-  if (polarity_reverse)
-    PWM = PWM * -1;
+    if (polarity_reverse)
+      PWM = PWM * -1;
     // Changing the motor direction based on the PWM value
-  if (PWM < 0)
-      digitalWrite(dirPin , LOW);
-  else 
-      digitalWrite(dirPin , HIGH);
+    if (PWM < 0)
+      digitalWrite(dirPin, LOW);
+    else
+      digitalWrite(dirPin, HIGH);
 
-  // generating the PWM pulses for changing the motor velocity
-    analogWrite(pwmPin , abs(PWM));
+    // generating the PWM pulses for changing the motor velocity
+    analogWrite(pwmPin, abs(PWM));
   }
 
-  void setPIDValue(float p , float i , float d )
+  void setPIDValue(float p, float i, float d)
   {
     kP = p;
     kI = i;
     kD = d;
-    pid.setGains(kP, kI , kD);
+    pid.setGains(kP, kI, kD);
   }
 
-  void setPIDOutput(int min , int max)
+  void setPIDOutput(int min, int max)
   {
     outputMin = min;
     outputMax = max;
-    pid.setOutputRange(outputMin , outputMax);
+    pid.setOutputRange(outputMin, outputMax);
   }
 
   void setCPR(long cpr)
@@ -132,45 +129,42 @@ class PositionControl
   void setAngle(float theta)
   {
     // input: motor speed in rotation per second
-    setPoint = long((theta* CPR)/360);
+    setPoint = long((theta * CPR) / 360);
   }
 
   float getAngle()
   {
-    return ((encoder->read()* 360)/double(CPR));
+    return ((encoder->read() * 360) / double(CPR));
   }
 
   void controlLoop()
-  { 
-  currentPosition = encoder->read();
-  pid.run();
-  if (setPoint <0)
-
-        if (((setPoint  + (tolerance*CPR)) > currentPosition) && ((setPoint  - (tolerance*CPR))) < currentPosition)
-        {
-          setMotorPWM( 0);
-          // Serial.println("Reached the target");
-        }
-      else
-        {
-          setMotorPWM(outputPWM);
-        }
-
-  else
-
-      if (((setPoint - (tolerance*CPR)) < currentPosition) && ((setPoint  + (tolerance*CPR)) > currentPosition))
-        {
-          setMotorPWM( 0);
-          // Serial.println("Reached the target");
-        }
-      else
-        {
-          setMotorPWM(outputPWM);
-        }
-
-  }
-
-
-};
+  {
+    currentPosition = encoder->read();
+    pid.run();
+    if (setPoint < 0)
+    {
+      if (((setPoint + (tolerance * CPR)) > currentPosition) && ((setPoint - (tolerance * CPR))) < currentPosition)
+      {
+        outputPWM = 0;
+        setPoint = encoder->read();
+        pid.stop();
+        // Serial.println("Reached the target");
+      }
+
+      setMotorPWM(outputPWM);
+    }
+    else
+    {
+      if (((setPoint - (tolerance * CPR)) < currentPosition) && ((setPoint + (tolerance * CPR)) > currentPosition))
+      {
+        outputPWM = 0;
+        setPoint = encoder->read();
+        pid.stop();
+        // Serial.println("Reached the target");
+      }
+
+      setMotorPWM(outputPWM);
+    }
+  };
 
 #endif

examples/SingleMotorWithROS/SingleMotorWithROS.ino

@@ -0,0 +1,83 @@
+#include "PositionControl.h"
+#include <Arduino.h>
+
+
+#include <ros.h>
+#include <std_msgs/Float32.h>
+
+
+
+PositionControl frontLeft;
+
+#define dirPin 9
+#define pwmPin 8
+#define encA 6
+#define encB 7
+
+
+int RATE = 50;
+float theta=0;
+ros::NodeHandle nh;
+std_msgs::Float32 position_angle;
+std_msgs::Float32 position_ticks;
+std_msgs::Float32 output_pwm;
+
+double *setpoint=&frontLeft.setPoint;
+
+void messageCb(const std_msgs::Float32 &state) {
+
+  theta = (float)state.data;
+  *setpoint = long((theta* frontLeft.CPR)/360);
+  nh.loginfo("data received");
+
+}
+ros::Publisher position_angle_pub("position_angle", &position_angle);
+ros::Publisher position_ticks_pub("position_ticks", &position_ticks);
+ros::Publisher output_pwm_pub("output_pwm", &output_pwm);
+ros::Subscriber<std_msgs::Float32>
+  position_sub("position_in", &messageCb);
+
+
+void setup() {
+
+  nh.getHardware()->setBaud(9600);
+  nh.initNode();
+  nh.subscribe(position_sub);
+
+  nh.advertise(position_angle_pub);
+  nh.advertise(position_ticks_pub);
+  nh.advertise(output_pwm_pub);
+
+
+  // initial microcontroller health indication
+  Serial1.begin(9600);
+
+  //setting motor pin parameters
+  frontLeft.setPin(12, 5, 2, 3);
+  frontLeft.setReversePolarity(false);
+  //Tolerance for error in control, have tolerance more than 0.01, otherwise motor inertia takes more time to reach the position with multiple oscillation
+  frontLeft.setTolerance(0.001);
+  // delay(5000);
+  frontLeft.setCPR(80548);
+  // delay(5000);
+  // frontLeft.setPIDOutput(30, 30);
+  // frontLeft.setAngle(90);
+  // frontLeft.setAngle(-90);
+}  
+
+void loop() {
+  frontLeft.controlLoop();
+  if (nh.connected()) {
+    position_angle.data=frontLeft.getAngle();
+    position_ticks.data=frontLeft.currentPosition;
+    output_pwm.data=frontLeft.outputPWM;
+
+    position_angle_pub.publish(&position_angle);
+    position_ticks_pub.publish(&position_ticks);
+    output_pwm_pub.publish(&output_pwm);
+  }
+
+  nh.spinOnce();
+  // Serial.println(frontLeft.getAngle());
+  // Serial.println(frontLeft.currentPosition);
+}