Lab AI4 การใช้ CiRA CORE สั่งงานบอร์ด Lotus Nano Bot ควบคุมมอเตอร์กระแสตรง

1. ทำการโหลด Code Arduino ide ข้างล่างลงบนบอร์ด Lotus Nano Bot

//คำสั่ง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]) : "";

}

DL1 =0 // พอร์ตดิจิตอลซ้ายตัวที่1

DL2 = 1 // พอร์ตดิจิตอลซ้ายตัวที่2

PWML = 128 // พอร์ต PWMซ้าย

DR1 = 0 // พอร์ตดิจิตอลขวาตัวที่1

DR2 =1 // พอร์ตดิจิตอลขวาตัวที่2

PWMR = 128 // พอร์ต PWMขวา