Dalam membuat robot saya memakai arduino sebagai mikrokontrollernya, banyak hal tersambung ke mikro ini. mis: sensor dan motor.
Dalam pengkodean arduino saya memakai cara yang praktis namun memakan banyak resource program. Yaitu menggunakan String sebagai alat transport serial.
setelah pembacaan sensor, semua value langsung di buatkan string seperti ini:
0:1:2:3:4:5:6:1:2:3:4#
langsung di send sekali jalan ke komputer, sama sekali nggak effisien resource, tapi program cepat jadi.
kekurangannya ialah : setiap delay(xxx) akan mempengaruhi kinerja.
maka dari itu pelajarilah contoh SerialEvent.ino yang dibundle dengan IDE arduino. dengan itu. anda dapat menggunakan serial untuk menerima karakter perintah, walaupun sensor tetap mengirim data ke komputer.
Sebelumnya saya memproses satu persatu. namun ada teknik program yang membuat serial menjadi lebih efisien.
Tetap belajar.
// ---------------------------------------------------------------------------
// This example code was used to successfully communicate with 15 ultrasonic sensors. You can adjust
// the number of sensors in your project by changing SONAR_NUM and the number of NewPing objects in the
// "sonar" array. You also need to change the pins for each sensor for the NewPing objects. Each sensor
// is pinged at 33ms intervals. So, one cycle of all sensors takes 495ms (33 * 15 = 495ms). The results
// are sent to the "oneSensorCycle" function which currently just displays the distance data. Your project
// would normally process the sensor results in this function (for example, decide if a robot needs to
// turn and call the turn function). Keep in mind this example is event-driven. Your complete sketch needs
// to be written so there's no "delay" commands and the loop() cycles at faster than a 33ms rate. If other
// processes take longer than 33ms, you'll need to increase PING_INTERVAL so it doesn't get behind.
// ---------------------------------------------------------------------------
#include
#include
// the commands needed for the SRF sensors:
#define sensorAddress 0x60
// this is the memory register in the sensor that contains the result:
#define resultRegister 0x02
#define SONAR_NUM 6 // Number or sensors.
#define MAX_DISTANCE 200 // Maximum distance (in cm) to ping.
#define PING_INTERVAL 33 // Milliseconds between sensor pings (29ms is about the min to avoid cross-sensor echo).
#define PING1_TR 51 // Arduino pin tied to trigger pin on the ultrasonic sensor.
#define PING1_EC 53 // Arduino pin tied to echo pin on the ultrasonic sensor.
#define PING2_TR 47
#define PING2_EC 49
#define PING3_TR 43
#define PING3_EC 45
#define PING4_TR 39
#define PING4_EC 41
#define PING5_TR 35
#define PING5_EC 37
#define PING6_TR 31
#define PING6_EC 33
#define buzzPin 13
//Arduino to EMS30 Pin
int MIN1_1 = 11;
int MIN1_2 = 10;
int MEN1_1 = 12;
int MEN1_2 = 9;
int MPWM1 = 8;
int MIN2_1 = 6;
int MIN2_2 = 5;
int MEN2_1 = 7;
int MEN2_2 = 4;
int MPWM2 = 3;
int PPWM1 = 0;
int PPWM2 = 0;
int SPEED = 0;
int DIR = 0;
String XTRA = "";
int pinBuzzer = 40;
unsigned int uS1 = 0;
unsigned int uS2 = 0;
unsigned int uS3 = 0;
unsigned int uS4 = 0;
unsigned int uS5 = 0;
unsigned int uS6 = 0;
float filterVal = .6;
float suS1 = 0;
float suS2 = 0;
float suS3 = 0;
float suS4 = 0;
float suS5 = 0;
float suS6 = 0;
float ir_filterVal = .4;
float iR1 = 0;
float iR2 = 0;
float iR3 = 0;
float iR4 = 0;
//cmps
int bearing = 0;
NewPing ping1(PING1_TR, PING1_EC, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
NewPing ping2(PING2_TR, PING2_EC, MAX_DISTANCE);
NewPing ping3(PING3_TR, PING3_EC, MAX_DISTANCE);
NewPing ping4(PING4_TR, PING4_EC, MAX_DISTANCE);
NewPing ping5(PING5_TR, PING5_EC, MAX_DISTANCE);
NewPing ping6(PING6_TR, PING6_EC, MAX_DISTANCE);
String inputString = ""; // a string to hold incoming data
boolean stringComplete = false; // whether the string is complete
void getInfraRed()
{
iR1 = smooth(averageAnalog(6),ir_filterVal, iR1);
iR2 = smooth(averageAnalog(5),ir_filterVal, iR2);
iR3 = smooth(averageAnalog(8),ir_filterVal, iR3);
iR4 = smooth(averageAnalog(7),ir_filterVal, iR4);
// iR1 = averageAnalog(6);
// iR2 = averageAnalog(5);
// iR3 = averageAnalog(8);
// iR4 = averageAnalog(7);
//delay(100);
}
// for smoothing value in Range
int smooth(int data, float filterVal, float smoothedVal){
if (filterVal > 1){ // check to make sure param's are within range
filterVal = .99;
}
else if (filterVal <= 0){
filterVal = 0;
}
smoothedVal = (data * (1 - filterVal)) + (smoothedVal * filterVal);
return (int)smoothedVal;
}
void errorBeep()
{
digitalWrite(buzzPin, HIGH);
delay(200);
digitalWrite(buzzPin, LOW);
delay(50);
digitalWrite(buzzPin, HIGH);
delay(200);
digitalWrite(buzzPin, LOW);
delay(50);
digitalWrite(buzzPin, HIGH);
delay(200);
digitalWrite(buzzPin, LOW);
}
void getSonar() {
uS1 = ping1.ping()/US_ROUNDTRIP_CM;
uS2 = ping2.ping()/US_ROUNDTRIP_CM;
uS3 = ping3.ping()/US_ROUNDTRIP_CM;
uS4 = ping4.ping()/US_ROUNDTRIP_CM;
uS5 = ping5.ping()/US_ROUNDTRIP_CM;
uS6 = ping6.ping()/US_ROUNDTRIP_CM;
if(uS1 == 0){ uS1 = int(suS1);}
if(uS2 == 0){ uS2 = int(suS2);}
if(uS3 == 0){ uS3 = int(suS3);}
if(uS4 == 0){ uS4 = int(suS4);}
if(uS5 == 0){ uS5 = int(suS5);}
if(uS6 == 0){ uS6 = int(suS6);}
//average both
suS1 = smooth(uS1,filterVal,suS1);
suS2 = smooth(uS2,filterVal,suS2);
suS3 = smooth(uS3,filterVal,suS3);
suS4 = smooth(uS4,filterVal,suS4);
suS5 = smooth(uS5,filterVal,suS5);
suS6 = smooth(uS6,filterVal,suS6);
}
int averageAnalog(int pin){
int v=0;
for(int i=0; i<4 analogread="analogread" br="br" i="i" pin="pin" v="v"> return v/4;
}
void doubleBeep()
{
digitalWrite(buzzPin, HIGH);
delay(300);
digitalWrite(buzzPin, LOW);
delay(50);
digitalWrite(buzzPin, HIGH);
delay(300);
digitalWrite(buzzPin, LOW);
}
void turnLeft(){
digitalWrite(MIN1_1,LOW); //go back
digitalWrite(MIN1_2, HIGH);
digitalWrite(MIN2_1,HIGH); //go back
digitalWrite(MIN2_2, LOW);
}
void turnRight(){
digitalWrite(MIN1_1,HIGH); //go back
digitalWrite(MIN1_2, LOW);
digitalWrite(MIN2_1,LOW); //go back
digitalWrite(MIN2_2, HIGH);
}
void goBackward(){
digitalWrite(MIN1_1,LOW); //maju
digitalWrite(MIN1_2, HIGH);
digitalWrite(MIN2_1,LOW); //maju
digitalWrite(MIN2_2, HIGH);
}
void goForward(){
digitalWrite(MIN1_1,HIGH); //go back
digitalWrite(MIN1_2, LOW);
digitalWrite(MIN2_1,HIGH); //go back
digitalWrite(MIN2_2, LOW);
}
void stopMe() {
digitalWrite(MIN1_1,LOW);
digitalWrite(MIN1_2, LOW);
digitalWrite(MIN2_1,LOW);
digitalWrite(MIN2_2, LOW);
digitalWrite(MPWM1, HIGH); //PWM Speed Control
digitalWrite(MPWM2, HIGH); //PWM Speed Control
}
void highMen(){
digitalWrite(MEN1_1,HIGH);
digitalWrite(MEN1_2, HIGH);
digitalWrite(MEN2_1,HIGH);
digitalWrite(MEN2_2, HIGH);
delay(30);}
void lowMen(){
digitalWrite(MEN1_1,LOW);
digitalWrite(MEN1_2, LOW);
digitalWrite(MEN2_1,LOW);
digitalWrite(MEN2_2, LOW);
delay(30);}
void setPPWM(){
analogWrite(MPWM1, PPWM1); //PWM Speed Control
analogWrite(MPWM2, PPWM2); //PWM Speed Control
}
void processString() {
String stringOne = inputString;
int firsty = stringOne.indexOf(":");
int secondy = stringOne.indexOf(":",firsty+1);
int thirdy = stringOne.indexOf(":",secondy+1);
int foury = stringOne.indexOf(":",thirdy+1);
int fivy = stringOne.length();
String onez = stringOne.substring(0,firsty);
String duez = stringOne.substring(firsty+1,secondy);
String thirdez = stringOne.substring(secondy+1,thirdy);
String fourdy = stringOne.substring(thirdy+1,foury);
String fivedy = stringOne.substring(foury+1,fivy);
SPEED = onez.toInt();
DIR = duez.toInt();
PPWM1 = thirdez.toInt();
PPWM2 = fourdy.toInt();
XTRA = fivedy;
}
void stopRobot(){
stopMe();
}
void setup() {
doubleBeep();
pinMode(MIN1_1, OUTPUT);
pinMode(MIN1_2, OUTPUT);
pinMode(MEN1_1, OUTPUT);
pinMode(MEN1_2, OUTPUT);
pinMode(MIN2_1, OUTPUT);
pinMode(MIN2_2, OUTPUT);
pinMode(MEN2_1, OUTPUT);
pinMode(MEN2_2, OUTPUT);
pinMode(MPWM1, OUTPUT);
pinMode(MPWM2, OUTPUT);
highMen(); // turn on MEN
Serial.begin(115200);
Wire.begin();
//
inputString.reserve(200);
Serial.println("Ready");
}
void loop() {
// Process the string when a newline arrives:
// print the string when a newline arrives:
if (stringComplete) {
// clear the string:
inputString = "";
stringComplete = false;
}
if (DIR == 1) {
goForward();
setPPWM();
}
if (DIR == 3) {
goBackward();
setPPWM();
}
if (DIR == 2) {
turnLeft();
setPPWM();
}
if (DIR == 4) {
turnRight();
setPPWM();
}
if (DIR == 0) {
stopRobot();
}
getInfraRed();
getSonar();
// send the CMPS command to read the result in degree:
setRegister(sensorAddress, resultRegister);
// read the result:
bearing = readData(sensorAddress, 2);
sendSerial();
}
void sendSerial(){
Serial.print("S:");
Serial.print(suS1);
Serial.print(":");
Serial.print(suS2);
Serial.print(":");
Serial.print(suS3);
Serial.print(":");
Serial.print(suS4);
Serial.print(":");
Serial.print(suS5);
Serial.print(":");
Serial.print(suS6);
Serial.print(":");
Serial.print(iR1);
Serial.print(":");
Serial.print(iR2);
Serial.print(":");
Serial.print(iR3);
Serial.print(":");
Serial.print(iR4);
Serial.print(":");
Serial.print(bearing/10);
Serial.println(":");
}
/*
SerialEvent occurs whenever a new data comes in the
hardware serial RX. This routine is run between each
time loop() runs, so using delay inside loop can delay
response. Multiple bytes of data may be available.
*/
void serialEvent() {
while (Serial.available()) {
// get the new byte:
char inChar = (char)Serial.read();
// add it to the inputString:
inputString += inChar;
// if the incoming character is a newline, set a flag
// so the main loop can do something about it:
if (inChar == '#') {
stringComplete = true;
processString();
}
}
}
/*
setRegister() tells the CMP sensor to change the address pointer position
*/
void setRegister(int address, int thisRegister) {
// start I2C transmission:
Wire.beginTransmission(address);
// send address to read from:
Wire.write(thisRegister);
// end I2C transmission:
Wire.endTransmission();
}
/*
readData() returns a result from the SRF sensor
*/
int readData(int address, int numBytes) {
int result = 0; // the result is two bytes long
// send I2C request for data:
Wire.requestFrom(address, numBytes);
// wait for two bytes to return:
while (Wire.available() < 2 ) {
// wait for result
}
// read the two bytes, and combine them into one int:
result = Wire.read() * 256;
result = result + Wire.read();
// return the result:
return result;
}
Tiap -tiap nodes terhubung melalui sebuah topik khusus. Nodes Camera mem'publish' Topik Landmark Visual. Isi dari Topik ini adalah data yang ditangkap oleh kamera. Namun data ini berguna bagi nodes visualisasi titik lokasi, nodes ini 'subscribe' ke topik tsb, mengambil datanya.. lalu mengolahnya menjadi data yang berguna untuk menentukan titik kordinat robot sekarang.
