pd_ros_escravo

#include <ros.h>
#include <std_msgs/Float64.h>


const int pwm = 11;
const int dir1 = 8;
const int dir2 = 9;
const int pot_pin = A0;
const int corr_pin = A1;

float angulo = 0;
float setpoint = 150;
float Kp = 5; // 10.3;
float Kd = 10;//0.0512;
float ultimo_erro = 0;
float erro = 0;
float delta_erro = 0;
float termo_pd = 0;
float termo_pd_escalonado = 0;
float mV = 0;
float Amp = 0;

int pot = 0;
int corrente = 0;
int mVperAmp = 66;
int ACSoffset = 2500;

ros::NodeHandle nh;

std_msgs::Float64 posi;
std_msgs::Float64 corr;

void setpoint_callback(const std_msgs::Float64& msg){
    setpoint = msg.data;
}

ros::Subscriber<std_msgs::Float64> ss("posicao_mestre", &setpoint_callback);

ros::Publisher p("posicao", &posi);
ros::Publisher c("corrente", &corr);

void setup() {
  nh.initNode();
  nh.advertise(p);
  nh.advertise(c);
  nh.subscribe(ss);

  pinMode(pwm, OUTPUT);
  pinMode(dir1, OUTPUT);
  pinMode(dir2, OUTPUT);
}

void loop(){
  pd_calc();
  pd_calc2();
  pd_calc3();
  pd_calc4();
  pd_calc5();
  pd_calc6();

  if (angulo < setpoint) {
    digitalWrite(dir1, LOW);
    digitalWrite(dir2, HIGH);
  }
  else {
    digitalWrite(dir1, HIGH);
    digitalWrite(dir2, LOW);
  }

  analogWrite(pwm, termo_pd_escalonado);

  posi.data = angulo;
  corr.data = Amp;

  p.publish( &posi );
  c.publish( &corr );

  nh.subscribe(ss);
  nh.spinOnce();

  delay(20);
}

void pd_calc(){
  pot = analogRead(pot_pin);
  angulo = map(pot, 0, 1023, 0, 180);
  corrente = analogRead(corr_pin);
  mV = (corrente / 1024.0) * 5000;
  Amp = ((mV - ACSoffset) / mVperAmp);

  if (angulo > 165 or angulo < 15){ //FREIO!!
    digitalWrite(dir1, LOW);
    digitalWrite(dir2, LOW);
    termo_pd_escalonado = 0;
  }
  else {
    erro = setpoint - angulo;
    delta_erro = erro - ultimo_erro;
    termo_pd = (Kp * erro) + (Kd * delta_erro);
    termo_pd = constrain(termo_pd, -255, 255);
    termo_pd_escalonado = abs(termo_pd);
    ultimo_erro = erro;
  }
}
void pd_calc2(){
  pot = analogRead(pot_pin);
  angulo = map(pot, 0, 1023, 0, 180);
  corrente = analogRead(corr_pin);
  mV = (corrente / 1024.0) * 5000;
  Amp = ((mV - ACSoffset) / mVperAmp);

  if (angulo > 165 or angulo < 15){ //FREIO!!
    digitalWrite(dir1, LOW);
    digitalWrite(dir2, LOW);
    termo_pd_escalonado = 0;
  }
  else {
    erro = setpoint - angulo;
    delta_erro = erro - ultimo_erro;
    termo_pd = (Kp * erro) + (Kd * delta_erro);
    termo_pd = constrain(termo_pd, -255, 255);
    termo_pd_escalonado = abs(termo_pd);
    ultimo_erro = erro;
  }
}
void pd_calc3(){
  pot = analogRead(pot_pin);
  angulo = map(pot, 0, 1023, 0, 180);
  corrente = analogRead(corr_pin);
  mV = (corrente / 1024.0) * 5000;
  Amp = ((mV - ACSoffset) / mVperAmp);

  if (angulo > 165 or angulo < 15){ //FREIO!!
    digitalWrite(8, LOW);
    digitalWrite(9, LOW);
    termo_pd_escalonado = 0;
  }
  else {
    erro = setpoint - angulo;
    delta_erro = erro - ultimo_erro;
    termo_pd = (Kp * erro) + (Kd * delta_erro);
    termo_pd = constrain(termo_pd, -255, 255);
    termo_pd_escalonado = abs(termo_pd);
    ultimo_erro = erro;
  }
}
void pd_calc4(){
  pot = analogRead(pot_pin);
  angulo = map(pot, 0, 1023, 0, 180);
  corrente = analogRead(corr_pin);
  mV = (corrente / 1024.0) * 5000;
  Amp = ((mV - ACSoffset) / mVperAmp);

  if (angulo > 165 or angulo < 15){ //FREIO!!
    digitalWrite(8, LOW);
    digitalWrite(9, LOW);
    termo_pd_escalonado = 0;
  }
  else {
    erro = setpoint - angulo;
    delta_erro = erro - ultimo_erro;
    termo_pd = (Kp * erro) + (Kd * delta_erro);
    termo_pd = constrain(termo_pd, -255, 255);
    termo_pd_escalonado = abs(termo_pd);
    ultimo_erro = erro;
  }
}
void pd_calc5(){
  pot = analogRead(pot_pin);
  angulo = map(pot, 0, 1023, 0, 180);
  corrente = analogRead(corr_pin);
  mV = (corrente / 1024.0) * 5000;
  Amp = ((mV - ACSoffset) / mVperAmp);

  if (angulo > 165 or angulo < 15){ //FREIO!!
    digitalWrite(8, LOW);
    digitalWrite(9, LOW);
    termo_pd_escalonado = 0;
  }
  else {
    erro = setpoint - angulo;
    delta_erro = erro - ultimo_erro;
    termo_pd = (Kp * erro) + (Kd * delta_erro);
    termo_pd = constrain(termo_pd, -255, 255);
    termo_pd_escalonado = abs(termo_pd);
    ultimo_erro = erro;
  }
}
void pd_calc6(){
  pot = analogRead(pot_pin);
  angulo = map(pot, 0, 1023, 0, 180);
  corrente = analogRead(corr_pin);
  mV = (corrente / 1024.0) * 5000;
  Amp = ((mV - ACSoffset) / mVperAmp);

  if (angulo > 165 or angulo < 15){ //FREIO!!
    digitalWrite(8, LOW);
    digitalWrite(9, LOW);
    termo_pd_escalonado = 0;
  }
  else {
    erro = setpoint - angulo;
    delta_erro = erro - ultimo_erro;
    termo_pd = (Kp * erro) + (Kd * delta_erro);
    termo_pd = constrain(termo_pd, -255, 255);
    termo_pd_escalonado = abs(termo_pd);
    ultimo_erro = erro;
  }
}