Lab AI6 การใช้ CiRA CORE สั่งงานบอร์ด Lotus Nano Bot ในการควบคุม Servo Motor

//คำสั่งTerminalในการเปิดพอร์ตUSB :sudo chmod 666 /dev/ttyUSB0

#if (ARDUINO >= 100)

 #include <Arduino.h>

#else

 #include <WProgram.h>

#endif

#include <VarSpeedServo.h> 

//#include <Servo.h> 

#include <ros.h>

#include <std_msgs/String.h>

//Lotus Nano Bot Shield

#define D0 0

#define D1 1

#define D2 2

#define D3 3 //buzzer

#define D4 13

#define DR1  8

#define DR2  7  

#define PWMR 6 //Motor Right

#define DL1  4  

#define DL2  9 

#define PWML 5 //Motor Left

int dl_1 = 0;

int dl_2 = 0;

int spl = 0;

int dr_1 = 0;

int dr_2 = 0;

int spr = 0;

String GetStringPartAtSpecificIndex(const String data, const char separator, int index);

ros::NodeHandle  nh;

//Servo servo1, servo2, servo3;

VarSpeedServo servo1, servo2, servo3;

//SV1 D10, SV2 D11, SV3 D12

//direction

int di1 = -1, di2 = -1, di3 = -1;

bool is_lock = false;

std_msgs::String str_msg;

ros::Publisher pub_servo("curr_servo", &str_msg);

void publishCurrServo() {

 String stri = 

    String((servo1.read() - 90) * di1) + "," 

  + String((servo2.read() - 90) * di2) + "," 

  + String((servo3.read() - 90) * di3) + "," 

  + String(analogRead(A0)) + "," 

  + String(analogRead(A1)) + "," 

  + String(analogRead(A2)) + "," 

  + String(analogRead(A3)) + ","

  + String(analogRead(A4)) + "," 

  + String(analogRead(A5)) + "," 

  + String(analogRead(A6)) + "," ;

//Plese delay for long string

  delay(5);

  stri += String(digitalRead(D2)) + ","

  + String(digitalRead(D0)) + ","

  + String(digitalRead(D1)) + ","

  + String(digitalRead(D3)) + ","

  + String(digitalRead(D4)) ;

  str_msg.data = &stri[0];

  pub_servo.publish( &str_msg );

}

void magnum_cb(const std_msgs::String& cmd_msg){

  String str;

  str = &cmd_msg.data[0];

  //servo individual

  if(str.indexOf('s') == 0) {

    int pos1_ = servo1.read(); 

    int pos2_ = servo2.read();

    int pos3_ = servo3.read();

    int vel1_ = 20, vel2_ = 20, vel3_ = 20;

    String pos;

    pos = GetStringPartAtSpecificIndex(str, ':', 1);

    if(pos != "") {

      pos1_ = (pos.toInt()*di1)  + 90;

      vel1_ = GetStringPartAtSpecificIndex(str, ':', 2).toInt();

    }

    pos = GetStringPartAtSpecificIndex(str, ':', 3);

    if(pos != "") {

      pos2_ = (pos.toInt()*di2)  + 90;

      vel2_ = GetStringPartAtSpecificIndex(str, ':', 4).toInt();

    }

    pos = GetStringPartAtSpecificIndex(str, ':', 5);

    if(pos != "") {

      pos3_ = (pos.toInt()*di3)  + 90;

      vel3_ = GetStringPartAtSpecificIndex(str, ':', 6).toInt();

    }

    servo1.write(pos1_, vel1_, false);

    servo2.write(pos2_, vel2_, false);

    servo3.write(pos3_, vel3_, false);

    return;

  }

  //dc motor 

  if(str.indexOf('m') == 0) {

    dl_1 = GetStringPartAtSpecificIndex(str, ':', 1).toInt();

    dl_2 = GetStringPartAtSpecificIndex(str, ':', 2).toInt();

    spl = GetStringPartAtSpecificIndex(str, ':', 3).toInt();

    dr_1 = GetStringPartAtSpecificIndex(str, ':', 4).toInt();

    dr_2 = GetStringPartAtSpecificIndex(str, ':', 5).toInt();

    spr = GetStringPartAtSpecificIndex(str, ':', 6).toInt();

    return;

  }

  //servo robot

  int pos1 = ( GetStringPartAtSpecificIndex(str, ',', 0).toInt() * di1)  + 90;

  int pos2 = ( GetStringPartAtSpecificIndex(str, ',', 1).toInt() * di2) + 90;

  int pos3 = ( GetStringPartAtSpecificIndex(str, ',', 2).toInt() * di3) + 90;

  if(pos1 < 0 || pos1 > 180 || pos2 < 0 || pos2 > 180 || pos3 < 0 || pos3 > 180) {

    return;  

  }

  int vel1 = GetStringPartAtSpecificIndex(str, ',', 3).toInt();

  int vel2 = GetStringPartAtSpecificIndex(str, ',', 4).toInt();

  int vel3 = GetStringPartAtSpecificIndex(str, ',', 5).toInt();

  servo1.write(pos1, vel1, false);

  servo2.write(pos2, vel2, false);

  servo3.write(pos3, vel3, false);

}

void oled_cb( const std_msgs::String& cmd_msg){

 //"1::WHITE,BLACK::0,0::1::hello"

}

void buzzer_cb( const std_msgs::String& cmd_msg){

  String str = String(cmd_msg.data);

  int freq = GetStringPartAtSpecificIndex(str, ':', 0).toInt();

  int timer_buzz = GetStringPartAtSpecificIndex(str, ':', 1).toInt();

  if(freq == -1) noTone(3);

  else tone(3, freq, timer_buzz);

}

void output_cb( const std_msgs::String& cmd_msg){

  String str = String(cmd_msg.data);

  //Digital write

  int d3val = GetStringPartAtSpecificIndex(str, ',', 2).toInt();

  int d4val = GetStringPartAtSpecificIndex(str, ',', 3).toInt();

  if(d3val != -1) digitalWrite(D3 ,d3val);

  if(d4val != -1) digitalWrite(D4 ,d4val);

}

ros::Subscriber<std_msgs::String> sub("servo", magnum_cb);

//ros::Subscriber<std_msgs::String> sub_oled("lotus_oled", oled_cb);

ros::Subscriber<std_msgs::String> sub_buzz("lotus_buzzer", buzzer_cb);

//ros::Subscriber<std_msgs::String> sub_motor_servo("lotus_motor_servo", motor_servo_cb);

ros::Subscriber<std_msgs::String> sub_out_cmd("output_cmd", output_cb);

void setup(){

  pinMode(D2, INPUT);

  pinMode(D3, OUTPUT);

  pinMode(D4, OUTPUT);

  pinMode(DL1, OUTPUT);

  pinMode(DL2, OUTPUT);

  pinMode(PWML, OUTPUT);

  pinMode(DR1, OUTPUT);

  pinMode(DR2, OUTPUT);

  pinMode(PWMR, OUTPUT);

  nh.initNode();

  nh.subscribe(sub);

  //nh.subscribe(sub_oled);

  nh.subscribe(sub_buzz);

  //nh.subscribe(sub_motor_servo);

  nh.advertise(pub_servo);

  nh.subscribe(sub_out_cmd);

  delay(10);

  servo1.attach(10);   // attach servo1

  delay(50);

  servo2.attach(11);  // attach servo2

  delay(50);

  servo3.attach(12);  // attach servo3

}

void loop(){

  //for ros control

  publishCurrServo();

  if(dl_1 == 0) digitalWrite(DL1, LOW);

  if(dl_1 == 1) digitalWrite(DL1, HIGH);

  if(dl_2 == 0) digitalWrite(DL2, LOW);

  if(dl_2 == 1) digitalWrite(DL2, HIGH);

  analogWrite(PWML, spl);

  if(dr_1 == 0) digitalWrite(DR1, LOW);

  if(dr_1 == 1) digitalWrite(DR1, HIGH);

  if(dr_2 == 0) digitalWrite(DR2, LOW);

  if(dr_2 == 1) digitalWrite(DR2, HIGH);

  analogWrite(PWMR, spr);

  nh.spinOnce();

  delay(5);

}

//#########################################################

String GetStringPartAtSpecificIndex(const String data, const char separator, int index)

{

    int found = 0;

    int strIndex[] = { 0, -1 };

    int maxIndex = data.length() - 1;

    for (int i = 0; i <= maxIndex && found <= index; i++) {

        if (data.charAt(i) == separator || i == maxIndex) {

            found++;

            strIndex[0] = strIndex[1] + 1;

            strIndex[1] = (i == maxIndex) ? i+1 : i;

        }

    }

    return found > index ? data.substring(strIndex[0], strIndex[1]) : "";

}