hey guys, i'm little bit frustate here. i'm trying to control dynamixel smart servo with an imu sensor gy 25. i successfully read the data from the gy 25 but when i combine the code with dynamixel program the sensor data in serial monitor is become unstable and having a lot of noise. i don't know why. i think it's because the parsing data form the gy 25 that crash with the dynamixel program. can someone help me please
btw here's my code
// Example 2 - Receive with an end-marker
const byte numChars = 32;
char receivedChars[numChars]; // an array to store the received data
boolean newData = false;
#include <DynamixelSerial1.h>
void setup() {
Dynamixel.begin(1000000,2);
delay(1000);
Serial.begin(115200); // Serial monitor
Serial3.begin(115200); // Serial GY25
delay(3000); // Jeda 3 detik
// Kalibrasi Tilt
Serial3.write(0xA5);
Serial3.write(0x54);
delay(1000); // Jeda sebelum kalibrasi heading
// Kalibrasi Heading
Serial3.write(0xA5);
Serial3.write(0x55);
delay(100); // Jeda sebelum konfigurasi output
// Output ASCII
Serial3.write(0xA5);
Serial3.write(0x53);
delay(100); // Jeda sebentar
Serial.println("");
}
void loop() {
recvWithEndMarker();
showNewData();
char prosesString[32];
strcpy(prosesString,receivedChars);
float Y,P,R;
Y = (prosesString[6]-'0')*100.00 + (prosesString[7]-'0')*10.00 + (prosesString[8]-'0')*1.00 + (prosesString[10]-'0')*0.10 +(prosesString[11]-'0')*0.01;
P = (prosesString[14]-'0')*100.00 + (prosesString[15]-'0')*10.00 + (prosesString[16]-'0')*1.00 + (prosesString[18]-'0')*0.10 + (prosesString[19]-'0')*0.01;
R = (prosesString[22]-'0')*100.00 + (prosesString[23]-'0')*10.00 + (prosesString[24]-'0')*1.00 + (prosesString[26]-'0')*0.10 + (prosesString[27]-'0')*0.01;
if (prosesString[5]=='-'){
Y = Y*(-1.00);
}
else {Y=Y;}
if (prosesString[13]=='-'){
P = P*(-1.00);
}
else {P=P;}
if (prosesString[21]=='-'){
R = R*(-1.00);
}
else {R=R;}
Serial.print("Y = ");
Serial.print(Y);
Serial.print("\t");
Serial.print("p = ");
Serial.print(P);
Dynamixel.moveSpeed(18,(P*1024/360)+512,1020);
Serial.print("\t");
Serial.print("R = ");
Serial.println(R);
return 0;
}
void recvWithEndMarker() {
static byte ndx = 0;
char endMarker = '\n';
char rc;
while (Serial3.available() > 0 && newData == false) {
rc = Serial3.read();
if (rc != endMarker) {
receivedChars[ndx] = rc;
ndx++;
if (ndx >= numChars) {
ndx = numChars - 1;
}
}
else {
receivedChars[ndx] = '\0'; // terminate the string
ndx = 0;
newData = true;
}
}
}
void showNewData() {
if (newData == true) {
// Serial.print("This just in ... ");
// Serial.println(receivedChars);
newData = false;
}
}
sketch_oct02d.ino (2.55 KB)