How to integrate rosserial_arduino in an existing sketch?
I made code in just arduino. but i wanna change it to rosserial_arduino code. but i don't know how to do.....
#include <SoftwareSerial.h>
SoftwareSerial hc06(10,11); //Tx, Rx
int total_state = 0;
int switch_on = 0;
int switch_rev = 0;
int switch_on_state = 0;
int switch_rev_state = 0;
int switch_on_pin = 3;
int switch_rev_pin = 4;
double emg ;
//double temp_max = 27;
double emg_max = 200;
double pres;
double pres_max = 150;
int mydelay = 300;
int relay_motor_pin = 9;
int pow_led_pin = 12;
int rev_led_pin = 13;
int baby_mot_pin = 6;
int direct_pin = 7;
unsigned long currentMillis;
unsigned long previousMillis = 0;
unsigned long changetime = 0;
//////////////////////
void pumpon(int relay_motor_pin) {
digitalWrite(relay_motor_pin, HIGH);
}
void pumpoff(int relay_motor_pin) {
digitalWrite(relay_motor_pin, LOW);
}
void pumprev(int LED_REV) {
digitalWrite(direct_pin,LOW);
analogWrite(baby_mot_pin,255);
delay(3000);
analogWrite(baby_mot_pin,0)
;
}
void serial_comu(int MYDELAY, double EMG, double PRES, int TOTAL_STATE, int SWITCH_ON_STATE) {
currentMillis = millis();
if (currentMillis - previousMillis > MYDELAY) {
hc06.print("= EMG = ");
hc06.print(EMG);
hc06.print("= PRES = ");
hc06.print(PRES);
hc06.print("= total state = ");
hc06.print(TOTAL_STATE);
hc06.print("= switch on state=");
hc06.print(SWITCH_ON_STATE);
hc06.println(" ");
previousMillis = currentMillis;
}
}
////////////////////////////////
void setup() {
Serial.begin(9600);
pinMode(relay_motor_pin, OUTPUT);
pinMode(switch_on_pin, INPUT);
pinMode(switch_rev_pin, INPUT);
pinMode(pow_led_pin,OUTPUT);
pinMode(rev_led_pin,OUTPUT);
pinMode(baby_mot_pin,OUTPUT);
pinMode(direct_pin,OUTPUT);
Serial.println("Enter AT commands:");
hc06.begin(9600);
}
void loop() {
if(hc06.available()){
Serial.write(hc06.read());
}
if(Serial.available()){
hc06.write(Serial.read());
}
emg = analogRead(A0);
pres = analogRead(A1);
switch_on = digitalRead(switch_on_pin);
switch_rev = digitalRead(switch_rev_pin);
if ( (millis() - changetime > 1000) && switch_on_state == LOW && switch_on == HIGH) {
switch_on_state = HIGH;
digitalWrite(pow_led_pin,HIGH);
changetime = millis();
}
if ( (millis() - changetime > 1000) && switch_on_state == HIGH && switch_on == HIGH) {
switch_on_state = LOW;
digitalWrite(pow_led_pin,LOW); //???????????????
changetime = millis();
}
if ( (millis() - changetime > 1000) && switch_rev_state == LOW && switch_rev == HIGH) {
switch_rev_state = HIGH;
digitalWrite(rev_led_pin,HIGH);
changetime = millis();
}
if ( (millis() - changetime > 1000) && switch_rev_state == HIGH && switch_rev == HIGH) {
switch_rev_state = LOW;
changetime = millis();
}
if ( (switch_on_state == HIGH) && (emg >= emg_max) && (pres <= pres_max) ) {
total_state = 1;
}
if (total_state == 1) {
pumpon(relay_motor_pin);
}
if ( (switch_on_state == LOW) || (pres > pres_max) ) {
total_state = 0;
}
if (total_state == 0) {
pumpoff(relay_motor_pin);
}
if (switch_rev_state == HIGH) {
total_state = 2;
}
if (total_state == 2) {
pumprev(relay_motor_pin);
digitalWrite(rev_led_pin,LOW);
total_state = 0;
switch_rev_state = LOW;
}
serial_comu(mydelay, emg, pres, total_state, switch_on_state);
}
Edit:
I saw rosserial_arduino tutorials. but i can't understand 100%... i delete bluetooth from code and i made these code
#include <ros.h>
#include <std_msgs/Float32.h>
std_msgs::Float32 EMG_msg;
ros::Publisher pub_EMG("EMG_sensor", &EMG_msg);
ros::NodeHandle nh;
int total_state = 0;
int switch_on = 0;
int switch_rev = 0;
int switch_on_state = 0;
int switch_rev_state = 0;
int switch_on_pin = 3;
int switch_rev_pin = 4;
double emg ;
//double temp_max = 27;
double emg_max =200;
double pres;
double pres_max = 130;
int mydelay = 300;
int led_motor_pin = 9;
unsigned long currentMillis;
unsigned long previousMillis = 0;
unsigned long changetime =0;
//////////////////////
void pumpon(int LED_ON){
digitalWrite(LED_ON,HIGH);
}
void pumpoff(int LED_OFF){
digitalWrite(LED_OFF,LOW);
}
void pumprev(int LED_REV){
digitalWrite(LED_REV,LOW);
/* currentMillis=millis();
if(currentMillis-previousMillis>1000){
digitalWrite(LED_REV,HIGH);
previousMillis=currentMillis;
}
currentMillis=millis();*/
delay(500);
digitalWrite(LED_REV,HIGH);
delay(500);
digitalWrite(LED_REV,LOW);
delay(500);
digitalWrite(LED_REV,HIGH);
delay(500);
digitalWrite(LED_REV,LOW);
}
void serial_comu(int MYDELAY, double EMG, double PRES, int TOTAL_STATE, int SWITCH_ON_STATE){
currentMillis=millis();
if(currentMillis-previousMillis>MYDELAY ...
So we're here to help, but not to do your work for you.
Please tell us what you've tried already and why that didn't work.
Finally -- and just to make sure: have you seen the rosserial_arduino tutorials? There's quite a few of them and they should give you an idea of what is possible.