Lab 0 Firmware Lotus Devkit v1.0 Code & Box control
Arduino IDE Code
//คำสั่งTerminalในการเปิดพอร์ตUSB :sudo chmod 666 /dev/ttyUSB0
#if (ARDUINO >= 100)
#include <Arduino.h>
#else
#include <WProgram.h>
#endif
#include <ESP32Servo.h>
//#include <VarSpeedServo.h>
//#include <Servo.h>
#include <ros.h>
#include <std_msgs/String.h>
//Lotus Nano Bot Shield
#define D0 1 // tx
#define D1 3 // rx
#define D2 27 // buttun
#define snd 18 //buzzer
#define D4 26 //
#define DR1 16
#define DR2 17
#define PWMR 4 //Motor Right
#define DL1 2
#define DL2 15
#define PWML 13 //Motor Left
int dl_1 = 0;
int dl_2 = 0;
int spl = 0;
int dr_1 = 0;
int dr_2 = 0;
int spr = 0;
///////////////////////////////////////////////////
int svv1=32; // servo ch1
int svv2=33; // servo ch2
int svv3=34; // servo ch3
int AA0=36;
int AA1=39;
int AA2=12;
int AA3=14;
int AA4=25;
int AA5=26;
int AA6=35; // k-nob
//////////////////////////////////////////////////
String GetStringPartAtSpecificIndex(const String data, const char separator, int index);
ros::NodeHandle nh;
//Servo servo1, servo2, servo3;
//VarSpeedServo servo1, servo2, servo3;
Servo 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(AA0)) + ","
+ String(analogRead(AA1)) + ","
+ String(analogRead(AA2)) + ","
+ String(analogRead(AA3)) + ","
+ String(analogRead(AA4)) + ","
+ String(analogRead(AA5)) + ","
+ String(analogRead(AA6)) + "," ;
//Plese delay for long string
delay(5);
stri += String(digitalRead(D2)) + ","
+ String(digitalRead(D0)) + ","
+ String(digitalRead(D1)) + ","
+ String(digitalRead(snd)) + ","
+ 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_);
servo2.write(pos2_);
servo3.write(pos3_);
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);
*/
servo1.write(pos1);
servo2.write(pos2);
servo3.write(pos3);
}
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(snd);
else tone(snd, 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(snd ,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(snd, 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(32); // attach servo1
servo1.attach(svv1, 500, 2400);
delay(50);
//servo2.attach(33); // attach servo2
servo2.attach(svv2, 500, 2400);
delay(50);
// servo3.attach(34); // attach servo3
servo3.attach(svv3, 500, 2400);
tone(snd,800,100);tone(snd,1000,100);tone(snd,800,100);
noTone(snd);
}
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