From e820d42e366708392ff867e4fe368d0f65158118 Mon Sep 17 00:00:00 2001 From: Jagadeeshan S Date: Mon, 30 Jan 2023 23:36:13 +0530 Subject: [PATCH] Added ros example and fixed overshooting bug --- PositionControl.h | 150 +++++++++--------- .../SingleMotorWithROS/SingleMotorWithROS.ino | 83 ++++++++++ 2 files changed, 155 insertions(+), 78 deletions(-) create mode 100644 examples/SingleMotorWithROS/SingleMotorWithROS.ino diff --git a/PositionControl.h b/PositionControl.h index 6b51b88..d2dced7 100644 --- a/PositionControl.h +++ b/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{¤tPosition, &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 diff --git a/examples/SingleMotorWithROS/SingleMotorWithROS.ino b/examples/SingleMotorWithROS/SingleMotorWithROS.ino new file mode 100644 index 0000000..dfdb90a --- /dev/null +++ b/examples/SingleMotorWithROS/SingleMotorWithROS.ino @@ -0,0 +1,83 @@ +#include "PositionControl.h" +#include + + +#include +#include + + + +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 + 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); +}