Lab 0 Firmware Lotus Devkit v1.0 Code & Box control

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

}