pd_disc_ros

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

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

int pot = 0, angulo = 0, corrente = 0;
float termo_pd_escalonado = 0, setpoint = 0;

ros::NodeHandle nh;

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

void contr_callback(const std_msgs::Float64& msg){
    termo_pd_escalonado = msg.data;
}

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

ros::Subscriber<std_msgs::Float64> sc("controle", &contr_callback);
ros::Subscriber<std_msgs::Float64> ss("setpt", &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);
  nh.subscribe(sc);

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

}

void loop() {
  pd_calc();

  if (angulo < setpoint) {
    digitalWrite(8, LOW);
    digitalWrite(9, HIGH);
  }
  else {
    digitalWrite(8, HIGH);
    digitalWrite(9, LOW);
  }

  analogWrite(11, termo_pd_escalonado);

  posi.data = angulo;
  corr.data = corrente;

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

  nh.subscribe(ss);
  nh.subscribe(sc);
  nh.spinOnce();
  delay(20);
}

void pd_calc(){
  pot = analogRead(pot_pin);
  angulo = map(pot, 0, 1023, 0, 180);
  corrente = analogRead(corr_pin);

  if (angulo > 165 or angulo < 15){ //FREIO!!!
    digitalWrite(8, LOW);
    digitalWrite(9, LOW);
    termo_pd_escalonado = 0;
  }
}