// the setup function runs once when you press reset or power the board
unsigned char headerBuf[14] = {0, };
unsigned char dataBuf[10] = {0,};
int motorIds[18] = {11,12,13,21,22,23,31,32,33,41,42,43,51,52,53,61,62,63};
char size = 0;
void setup() {
// initialize digital pin LED_BUILTIN as an output.
Serial.begin(115200);
while (!Serial){
// wait for serial port to connect. Needed for native USB port only
}
randomSeed(analogRead(0));
}
// the loop function runs over and over again forever
void loop() {
/////////////////// 피드백 요청 테스트 코드 //////////////
if (Serial.available()) {
Serial.readBytes(headerBuf, 4);
size = headerBuf[3];
Serial.readBytes(dataBuf, size);
memcpy(headerBuf + 4, dataBuf, size);
if((headerBuf[5] >= 0xA0 && headerBuf[5] <= 0xA8) ){
unsigned char ccw = 0x00;
unsigned char checkSum = ~(headerBuf[2] + 0x08 + 0x01 + headerBuf[2] + 125 + ccw);
unsigned char answer[12] = {0xFF, 0xFE,headerBuf[2], 0x08, checkSum , 0x01, ccw, 0, headerBuf[2], 125, 0, 0};
/* 43번 오류값 출력 테스트 */
/*
if(headerBuf[2] == (unsigned char)43){
unsigned char answer2[12] = {0xFF, 0xFE,headerBuf[2], 0x08, checkSum , 0x01, 0x00, 0, 244, 255, 0, 0};
memcpy(answer, answer2, 12);
}
*/
/* 43번 오류값 출력 테스트 끝 */
//unsigned char answer[12] = {0xFF, 0xFE,headerBuf[2], 0x07, 0xFF , 0x01, 0x00, random(255), 0, 125, 0, 0};
delay(10);
Serial.write(answer, 12);
}
}
}
//////////////피드백 테스트 코드/////////////
////////// 제어 에코 테스트 코드 ////////////
/*
if (Serial.available()) {
Serial.readBytes(headerBuf, 11);
Serial.write(headerBuf, 11);
}
*/
////////// 제어 에코 테스트 코드 end ////////////
///////특정 모터 무응답 대응 테스트
/*
if (Serial.available()) {
Serial.readBytes(headerBuf, 4);
size = headerBuf[3];
Serial.readBytes(dataBuf, size);
memcpy(headerBuf + 4, dataBuf, size);
if(((headerBuf[5] >= 0xA0 && headerBuf[5] <= 0xA8)) && headerBuf[2] != 0x1f ){ //31번에서 무응답..!
unsigned char answer[11] = {0xFF, 0xFE,headerBuf[2], 0x07, 0xFF , 0x01, 0x00, 0, headerBuf[2], 125, 0};
//unsigned char answer[11] = {0xFF, 0xFE,headerBuf[2], 0x07, 0xFF , 0x01, 0x00, random(255), 0, 125, 0};
delay(20);
Serial.write(answer, 11);
}
}
}*/
///////특정 모터 무응답 대응 테스트 끝