Lab 0 Library และ Code Firmware Arduino IDE สำหรับการใช้งานร่วมกับPlatform CiRA CORE

ดาวน์โหลดตัวอย่างCode และ Libraries ได้ที่  https://git.cira-lab.com/cira/cira-modules

#if (ARDUINO >= 100)

 #include <Arduino.h>

#else

 #include <WProgram.h>

#endif

#include <VarSpeedServo.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 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);

VarSpeedServo servo1, servo2, servo3;

bool is_lock = false;

void publishStatus() {

 String stri = 

  String(analogRead(A0)) + "," 

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

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

  delay(1);

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

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

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

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

  delay(1);

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

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

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

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

  + String(digitalRead(D4)) ;

  Serial.println(stri);

}

void magnum_cb(String str){

  //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())  + 90;

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

    }

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

    if(pos != "") {

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

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

    }

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

    if(pos != "") {

      pos3_ = (pos.toInt())  + 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;

  }

}

void oled_cb(String str){

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

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

 int is_black_white = GetStringPartAtSpecificIndex(str, ':', 2) ==  "BLACK,WHITE";

 int x = GetStringPartAtSpecificIndex( GetStringPartAtSpecificIndex(str, ':', 3), ",", 0).toInt();

 int y = GetStringPartAtSpecificIndex( GetStringPartAtSpecificIndex(str, ':', 3), ",", 1).toInt();

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

 String text = GetStringPartAtSpecificIndex(str, ':', 5);

}

void buzzer_cb(String str){

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

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

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

  else tone(3, freq, timer_buzz);

}

void output_cb(String str){

  //Digital write

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

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

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

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

}

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);

  Serial.begin(9600);

  delay(10);

  servo1.attach(10);   // attach servo1

  delay(50);

  servo2.attach(11);  // attach servo2

  delay(50);

  servo3.attach(12);  // attach servo3

}

void loop(){

  if (Serial.available() > 0) {

    String cmd  = Serial.readStringUntil('\n');

    if(cmd.indexOf("o:") == 0) { output_cb(cmd); Serial.println("o"); } 

    else if(cmd.indexOf("b:") == 0) { buzzer_cb(cmd); Serial.println("b"); }

    else if(cmd.indexOf("s:") == 0 || cmd.indexOf("m:") == 0) { magnum_cb(cmd); Serial.println(cmd[0]); }

    else if(cmd.indexOf("oled:") == 0) { oled_cb(cmd); Serial.println("oled"); }

    //Serial.println(cmd);

    //delay(5);

  }

  publishStatus();

  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);

  delay(1);

}

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

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]) : "";

}