#include <wiringPi.h>
#include <stdio.h>
#include <string.h>
#include <wiringSerial.h>
unsigned char sendbuf[32];
int i = 0;
int fd;
void move(int sId, int sPos, int sTime) {
unsigned char sum;
// パケット作成
sendbuf[0] = (unsigned char) 0xFA; // ヘッダ1
sendbuf[1] = (unsigned char) 0xAF; // ヘッダ2
sendbuf[2] = (unsigned char) sId; // サーボID
sendbuf[3] = (unsigned char) 0x00; // フラグ
sendbuf[4] = (unsigned char) 0x1E; // アドレス(0x1E=30)
sendbuf[5] = (unsigned char) 0x04; // 長さ(4byte)
sendbuf[6] = (unsigned char) 0x01; // 個数
sendbuf[7] = (unsigned char) (sPos & 0x00FF); // 位置
sendbuf[8] = (unsigned char) ((sPos & 0xFF00) >> 8); // 位置
sendbuf[9] = (unsigned char) (sTime & 0x00FF); // 時間
sendbuf[10] = (unsigned char) ((sTime & 0xFF00) >> 8); // 時間
// チェックサムの計算
sum = sendbuf[2];
for (int i = 3; i < 11; i++){
sum = (unsigned char)(sum ^ sendbuf[i]);
}
sendbuf[11] = sum; // チェックサム
// 送信
/*
digitalWrite(txden, HIGH);
Serial.write(sendbuf, 12);
delay(50);//送信が終わるまで待つ
digitalWrite(txden,LOW);
*/
for(i= 0;i<12;i++){
serialPutchar(fd,sendbuf[i]);
}
}
void torque(int sId, int sMode) {
unsigned char sum;
// パケット作成
sendbuf[0] = (unsigned char) (0xFA); // ヘッダー1
sendbuf[1] = (unsigned char) (0xAF); // ヘッダー2
sendbuf[2] = (unsigned char) (sId); // サーボID
sendbuf[3] = (unsigned char) (0x00); // フラグ
sendbuf[4] = (unsigned char) (0x24); // アドレス(0x24=36)
sendbuf[5] = (unsigned char) (0x01); // 長さ(4byte)
sendbuf[6] = (unsigned char) (0x01); // 個数
sendbuf[7] = (unsigned char)((sMode&0x00FF)); // ON/OFFフラグ
// チェックサムの計算
sum = sendbuf[2];
for (int i = 3; i < 8; i++) {
sum = (unsigned char) (sum ^ sendbuf[i]);
}
sendbuf[8] = sum; // チェックサム
// 送信
/* digitalWrite(txden, HIGH);
Serial.write(sendbuf, 9);
delay(50);//送信が終わるまで待つ
digitalWrite(txden, LOW);
*/
for(i=0;i<9;i++){
serialPutchar(fd,sendbuf[i]);
}
}
// Main function
int main(void) {
fd = serialOpen("/dev/ttyUSB0",115200); //baudrate=9600bps
if(fd<0){
printf("can not open serialport");
}
// Initialize WiringPi
if(wiringPiSetupGpio() == -1) return 1;
torque(1,1);
torque(2,1);
while(1){
move(1,500,0);
move(2,100,0);
delay(1000);
move(1,0,0);
move(2,0,0);
delay(1000);
}
return 0;
}
