Lab 3.2 การใช้งาน CiRA Lotus Nano Bot

Code สำหรับการใช้งานใน Arduino IDE (พัฒนาโดยทีม : AMI)

Arduino IDE Code

//คำสั่ง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 7 #define DR2 8 #define PWMR 6 //Motor Right#define DL1 9 #define DL2 4 #define PWML 5 //Motor Leftint 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//directionint 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]) : "";}

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