Arduino.ru

Извиняюсь, а Вы в этой программе школьник или педагог? Неужели нельзя сначала познакомиться с правилами оформления сообщения, а потом уж задавать вопросы? Изучайте: http://arduino.ru/forum/obshchii/vstavka-programmnogo-koda-v-temukommentarii

p.s. GET_HIGH_Byte и GET_HIGH_BYTE - это две большие разницы

Если по Гамбургскому счету, то три: y-Y, t-T, e-E. ;)) Итого три большие разницы!

Извините, вот код

#include<HardwareSerial.h>

#define MCU_TX_CON 25
#define MCU_TX  26
#define MCU_RX_CON 12
#define MCU_RX  35

#define GET_LOW_BYTE(A) (uint8_t)((A))
#define GET_HIGH_Byte(A) (uint8_t)((A)>>8)
#define BYTE_TO_HW(A,B) ((((uint16_t)(A))<<8)|(uint8_t)(B))

#define SERVO_FRAME_HEADER  0x55
#define SERVO_MOVE_TIME_WRITE 1
#define SERVO_MOVE_TIME_READ 2
#define SERVO_MOVE_TIME_WAIT_WRITE 7
#define SERVO_MOVE_TIME_WAIT_READ 8
#define SERVO_MOVE_START 11
#define SERVO_MOVE_STOP 12
#define SERVO_ID_WRITE 13
#define SERVO_ID_READ 14
#define SERVO_ANGLE_OFFSET_ADJUST 17
#define SERVO_ANGLE_OFFSET_WRITE 18
#define SERVO_ANGLE_OFFSET_READ 19
#define SERVO_ANGLE_LIMIT_WRITE 20
#define SERVO_ANGLE_LIMIT_READ 21
#define SERVO_TEMP_MAX_LIMIT_WRITE 24
#define SERVO_VIN_LIMIT_WRITE 22
#define SERVO_VIN_LIMIT_READ 23
#define SERVO_TEMP_MAX_LIMIT_READ 25
#define SERVO_TEMP_READ 26
#define SERVO_VIN_READ 27
#define SERVO_POS_READ 28
#define SERVO_OR_MOTOR_MODE_WRITE 29
#define SERVO_OR_MOTOR_MODE_READ 30
#define SERVO_LOAD_OR_UNLOAD_WRITE 31
#define SERVO_LOAD_OR_UNLOAD_READ 32
#define SERVO_LED_CTRL_WRITE 33
#define SERVO_LED_CTRL_READ 34
#define SERVO_LED_ERROR_WRITE 35
#define SERVO_LED_ERROR_READ 36

#define DEBUG 1

byte CheckSum(byte buf[])
{
 byte i;
 uint16_t temp=0;
 for(i=2;i<buf[3]+2; i++) {
temp +=buf[i];
}
temp= ~temp;
i= (byte)temp;
return i;
}
void SerialServoMove(HardwareSerial &SerialX, uint8_t id, int16_t position, uint16_t time)
{
byte buf[10];
if(position<0)
position = 0;
if(position>1000)
position = 1000;
buf[0]=buf[1]=SERVO_FRAME_HEADER;
buf[2]=id;
buf[3]=7;
buf[4]=SERVO_MOVE_TIME_WRITE;
buf[5]=GET_LOW_BYTE(position);
buf[6]=GET_HIGH_BYTE(position);
buf[7]=GET_LOW_BYTE(time);
but[8]=GET_HIGH_BYTE(time);
buf[9]=CheckSum(buf);
SerialX.write(buf, 10);
}

void SerialServoStopMove(HardwareSerial &SerialX, uint8_t id)
{
byte buf[6];
buf[0]=buf[1]=SERVO_FRAME_HEADER;
buf[2]=id;
buf[3]=3;
buf[4]=SERVO_MOVE_STOP;
buf[5]=CheckSum(buf);
SerialX.write(buf, 6);
}

void LobotSerialServoSetID(HardwareSerial &SerialX, uint8_t oldID, uint8_t newID)
{
byte buf[7];
buf[0]=buf[1]=SERVO_FRAME_HEADER;
buf[2]=oldID;
buf[3]=4;
buf[4]=SERVO_ID_WRITE;
buf[5]=newID;
buf[6]=CheckSum(buf);
SerialX.write(buf, 7);


#ifdef DEBUG
serial.println("SERVO ID_WRITE");
int debug_value_i=0;
for(debug_value_i=0; debug_value_i<buf[3]+3; debuge_value_i++)
{
Serial.print(buf[debug_value_i], HEX);
Serial.print(":");
}
Serial.println(" ");
#endif
}

void SerialServoSetMode(HardwareSerial &SerialX, uint8_t id,uint8_t Mode, int16_t Speed)
{
byte buf[10];

buf[0]=buf[1]=SERVO_FRAME_HEADER;
buf[2]=id;
buf[3]=7;
buf[4]=SERVO_OR_MOTOR_MODE_WRITE;
buf[5]=Mode;
buf[6]=0;
buf[7]=GET_LOW_BYTE((uint16_t)Speed);
buf[8]=GET_HIGH_BYTE((uint16_t)Speed);
buf[9]=CheckSum(buf);

#ifdef DEBUG
Serial.println("LOBOT SERVO Set Mode");
int debug_value_i=0;
for (debug_value_i=0; debug_value_i<buf[3]+3; debug_value_i++)
{
Serial.print(buf[debug_value_i], HEX);
Serial.print(":");
}
Serial.println(" ");
#endif

SerialX.write(buf, 10);
}
void SerialServoLoad(HardwareSerial &SerialX, uint8_t id)
{
byte buf[7];
buf[0]=buf[1]=SERVO_FRAME_HEADER;
buf[2]=id;
buf[3]=4;
buf[4]=SERVO_LOAD_OR_UNLOAD_WRITE;
buf[5]=1;
buf[6]=CheckSum(buf);

SerialX.write(buf, 7);

#ifdef DEBUG
Serial.println("SERVO LOAD WRITE");
int debug_value_i=0;
for (debug_value_i=0; debug_value_i<buf[3]+3; debug_value_i++)
{
Serial.print(buf[debug_value_i], HEX);
Serial.print(":");
}
Serial.println(" ");
#endif;

}

void SerialServoUnload(HardwareSerial &SerialX, uint8_t id)
{
byte buf[7];
buf[0]=buf[1]=SERVO_FRAME_HEADER;
buf[2]=id;
buf[3]=4;
buf[4]=SERVO_LOAD_OR_UNLOAD_WRITE;
buf[5]=0;
buf[6]=CheckSum(buf);

SerialX.write(buf, 7);

#ifdef DEBUG
Serial.println("SERVO LOAD WRITE");
int debug_value_i=0;
for (debug_value_i=0; debug_value_i<buf[3]+3; debug_value_i++)
{
Serial.print(buf[debug_value_i], HEX);
Serial.print(":");
}
Serial.print(" ");
#endif
}


int SerialServoReceiveHandle(HardwareSerial &SerialX, byte *ret)
{
bool frameStarted=false;
bool receiveFinished=false;
byte frameCount=0;
byte dataCount=0;
byte dataLength=2;
byte rxBuf;
byte recvBuf[32];
byte i;

while (SerialX.available()) {
rxBuf=SerialX.read();
delayMicroseconds(100);
if (!frameStarted){
if (rxBuf==SERVO_FRAME_HEANDER){

frameCount++;
if (frameCount==2) {
frameCount=0;
frameStarted=true;
dataCount=1;
}
}
else {
frameStarted=false;
dataCount=0;
frameCount=0;
}
}
if (frameStarted) {
recvBuf[dataCount]=(uint8_t)rxBuf;
if (dataCount==3) {
dataLength=recvBuf[dataCount];
if (datalength<3||dataCount>7){
dataLength=2;
frameStarted=false;
}
}
dataCount++;
if (dataCount==dataLength+3) {

#ifdef DEBUG
Serial.print("RECEIVE DATA:");
for (i=0; i<dataCount; i++){
Serial.print(recvBuf[i], HEX);
Serial.print(":");
}
serial.println(" ");
#endif

if (CheckSum(recvBuf)==recvBuf[dataCount - 1]) {

#ifdef DEBUG
Serial.println("Check SUM OK!!");
Serial.println("");
#endif

frameStarted=false;
memcpy(ret, recvBuf+4, dataLength);
return 1;
}
return -1;
}
}
}
}
int SerialServoReadPosition(HardwareSerial &SerialX, uint8_t id)
{
int count=10000;
int ret;
byte buf[6];

buf[0]=buf[1]=SERVO_FRAME_HEADER;
buf[2]=id;
buf[3]=3;
buf[4]=SERVO_POS_READ;
buf[5]=CheckSum(buf);

#ifdef DEBUG
Serial.println("SERVO Pos READ");
int debug_value_i=0;
for (debug_value_i=0; debug_value_i<buf[3]+3; debug_value_i++)
{
Serial.print(buf[debug_value_i], HEX);
Serial.print(":");
}
Serial.println(" ");
#endif

while (SerialX.available())
SerialX.read()

SerialX.write(buf, 6);

while (!Serial.available()) {
count-=1;
if (count<0)
return -2048;
}

if (SerialServoReceiveHandle(SerialX, buf)>0
ret=(int16_t)BYTE_TO_HW(buf[2], buf[1]);
else
ret=-2048;

#ifdef DEBUG
Serial.println(ret);
#endif
return ret;
}
int SerialServoReadVin(HardwareSerial &SerialX, uint8_t id)
{
int count=10000;
int ret;
byte buf[6];

buf[0]=buf[1]=SERVO_FRAME_HEADER;
buf[2]=id;
buf[3]=3;
buf[4]=SERVO_VIN_READ;
buf[5]=CheckSum(buf);

#ifdef DEBUG
Serial.println("SERVO VIN READ");
int debug_value_i=0;
for(debug_value_i=0; debug value_i<buf[3]+3; debug_value_i++)
{
Serial.print(buf[debug_value_i], HEX);
Serial.print(":");
}
Serial.println(" ");
#endif

while (SerialX.avilable())
SerialX.read();

SerialX.write(buf, 6);

while (!SerialX.available()) {
count-=1;
if (count<0)
return -2048;
}

if (SerialServoReceiveHandle(SerialX, buf)>0)
ret=(int16_t)BYTE_TO_HW(buf[2], buf[1]);
else
ret=-2049;

#ifdef DEBUG
Serial.println(ret);
#endif
return ret;
}

void setup() {
Serial.begin(115200);
Serial2.begin(115200, SERIAL_8N1, MCU_RX, MCU_TX);
pinMode(MCU_TX_CON,OUTPUT);
digitalWrite(MCU_TX_CON, HIGH);
pinMode(MCU_RX_CON,OUTPUT);
digitalWrite(MCU_RX_CON, LOW);
delay(1000);
}

#define ID1  1
#define ID2  2
#define ID3  3

void loop() {
SerialServoMove(Serial2,ID1, 100, 500);
SerialServoMove(Serial2,ID2, 500, 500);
SerialServoMove(Serial2,ID3, 500, 500);
delay(500);
SerialServoMove(Serial2,ID1, 500, 500);
SerialServoMove(Serial2,ID2, 600, 500);
SerialServoMove(Serial2,ID3, 100, 500);
delay(500);
SerialServoMove(Serial2,ID1, 900, 500);
SerialServoMove(Serial2,ID2, 700, 500);
SerialServoMove(Serial2,ID3, 700, 500);
delay(500);
SerialServoMove(Serial2,ID1, 500, 500);
SerialServoMove(Serial2,ID2, 600, 500);
SerialServoMove(Serial2,ID3, 100, 500);
delay(500);
}

9-ю стоку поправьте на ...BYTE

судя по отсутствию реакции на первое сообщение - автор абсолютно по нулям. Иначе бы просто исправил опечатку и даже код не нужно было бы выкладывать...

Извините ребята. Оказывается тут куча опечатков.