Kondo's SerialServo(ICS3.5) control sample for GR-SAKURA

gr_sketch.cpp

#include <rxduino.h>
#include <iodefine_gcc63n.h>

void sendCommand(unsigned char *data, unsigned short len)
{
    Serial.write((const unsigned char*)data, len);
    delay(2);
}

void setServoPos(unsigned char ServoID, unsigned short pos)
{
    byte cmd[3];
    cmd[0] = 0x80 | ServoID;
    cmd[1] = (unsigned char)((pos >> 7) & 0x7f);
    cmd[2] = (unsigned char)(pos & 0x7f);
    sendCommand(cmd, 3);
}

void setup()
{
    Serial1.begin(9600,SCI_SCI2B);
    Serial1.setDefault(); // printfをSerialに出力

    Serial.begin( 115200, SCI_SCI0P2x  );
    //SCI0 偶数パリティ有効
    SCI0.SCR.BIT.TE = 0;
    SCI0.SCR.BIT.RE = 0;
    SCI0.SMR.BIT.PE = 1;
    SCI0.SMR.BIT.PM = 0;
    SCI0.SCR.BIT.TE = 1;
    SCI0.SCR.BIT.RE = 1;
}

int x = 0;
void loop()
{
    if(x == 0){
        setServoPos(1, 2500);
        setServoPos(2, 12500);
        x = 1;
    }else{
        setServoPos(1, 12500);
        setServoPos(2, 2500);
        x = 0;
    }
    delay(1000);
}