int in1 = 8; //ポイント1スイッチ=8PIN int in2 = 9; //ポイント2スイッチ=9PIN int in0 = 7; //反転スイッチ=7PIN int in3 = 4; //ポイント3スイッチ=4PIN int in4 = 5; //ポイント4スイッチ=5PIN int in5 = 6; //ポイント5スイッチ=6PIN short previous_position = 0; //サーボ以前の位置 long rotate_speed = 2000; //1周移動時間(秒×100) //トルクONコマンド byte torque_on[] = {0xFA, 0xAF, 0x01, 0x00, 0x24, 0x01, 0x01, 0x01, 0x24}; // ポジションコマンド //byte position_data[] = {0xFA, 0xAF, 0x01, 0x00, 0x1E, 0x04, 0x01, 0x00, 0x00, 0x64, 0x00, 0x7E}; byte position_data[] = {0xFA, 0xAF, 0x01, 0x00, 0x1E, 0x04, 0x01, 0x00, 0x00, 0xF4, 0x01, 0xEF}; void memcpy(byte* buf, byte* data, int n) // データ転送 { int sum = 0; for(int i = 0; i < n; i++){ buf[i] = data[i]; } } void checksum(byte* data, int n) // チェックサムの設定 { int sum = 0; for(int i = 2; i < n - 1; i++){ sum = sum ^ data[i]; } data[ n-1 ] = sum; } void setup() { byte data[9]; // デジタル入力のプルアップ抵抗を有効にする pinMode(in1, INPUT_PULLUP); pinMode(in2, INPUT_PULLUP); pinMode(in0, INPUT_PULLUP); pinMode(in3, INPUT_PULLUP); pinMode(in4, INPUT_PULLUP); pinMode(in5, INPUT_PULLUP); Serial.begin(115200); //115200bpsでポートを開く delay(500); //0.5秒待つ Serial.write(torque_on,9); //サーボトルクON delay(500); //0.5秒待つ Serial.write(position_data,12); //原点まで回転 } void loop() { short current_position = previous_position; boolean reverse = digitalRead(in0) == LOW; if (digitalRead(in1) == LOW) { if (reverse) { current_position = -660; //停止位置1の反転位置設定 } else { current_position = 1176; //停止位置1の位置設定 } } if (digitalRead(in2) == LOW) { if (reverse) { current_position = -930; //停止位置2の反転位置設定 } else { current_position = 900; //停止位置2の位置設定 } } if (digitalRead(in3) == LOW) { if (reverse) { current_position = -444; //停止位置3の反転位置設定 } else { current_position = 1390; //停止位置3の位置設定 } } if (digitalRead(in4) == LOW) { if (reverse) { current_position = -585; //停止位置4の反転位置設定 } else { current_position = 1252; //停止位置4の位置設定 } } if (digitalRead(in5) == LOW) { if (reverse) { current_position = -340; //停止位置5の反転位置設定 } else { current_position = 1478; //停止位置5の位置設定 } } if (previous_position != current_position) { byte data[12]; //今回の移動時間を計算 short move_speed = short(abs((long) previous_position - (long) current_position) * rotate_speed / 3600); memcpy(data, position_data, 12); //回転データ転送 memcpy(&data[7], ¤t_position, 2); //角度設定 memcpy(&data[9], &move_speed, 2); //移動時間設定 checksum(data, 12); //チェックサム計算 Serial.write(data,12); //変更位置まで回転 previous_position = current_position; } delay(100); //スイッチチェック間隔 }