int in0 = 7; //反転スイッチ=7PIN int in1 = 8; //1番線スイッチ=8PIN int in2 = 9; //2番線スイッチ=9PIN int in3 = 4; //3番線スイッチ=4PIN int in4 = 5; //4番線スイッチ=5PIN int in5 = 6; //5番線スイッチ=6PIN byte servo_id = 1; //サーボID int previous_position = 0; //サーボ以前の位置 int rotate_speed = 2000; //1周移動時間(秒×100) int foward_offset = 0; //正回転補正値 int reverse_offset = -10; //逆回転補正値 byte torque_on[] = {0xFA, 0xAF, 0x01, 0x00, 0x24, 0x01, 0x01, 0x01, 0x24}; //トルクONコマンド byte torque_off[] = {0xFA, 0xAF, 0x01, 0x00, 0x24, 0x01, 0x01, 0x00, 0x25}; //トルクOFFコマンド byte position_data[] = {0xFA, 0xAF, 0x01, 0x00, 0x1E, 0x04, 0x01, 0x00, 0x00, 0x64, 0x00, 0x7E}; // 移動コマンド(時間指定) byte compliance_data[] = {0xFA, 0xAF, 0x01, 0x00, 0x18, 0x06, 0x01, 0x01, 0x01, 0x02, 0x02, 0xC4, 0x09, 0x82}; //コンプライアンススロープ2度 パンチ25% 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() { // デジタル入力のプルアップ抵抗を有効にする pinMode(in1, INPUT_PULLUP); pinMode(in2, INPUT_PULLUP); pinMode(in0, INPUT_PULLUP); pinMode(in3, INPUT_PULLUP); pinMode(in4, INPUT_PULLUP); pinMode(in5, INPUT_PULLUP); torque_on[2] = servo_id; checksum(torque_on, 9); //チェックサム計算 torque_off[2] = servo_id; checksum(torque_off, 9); //チェックサム計算 position_data[2] = servo_id; compliance_data[2] = servo_id; checksum(compliance_data, 14); //チェックサム計算 Serial.begin(115200); //115200bpsでポートを開く delay(500); //0.5秒待つ Serial.write(compliance_data,14); delay(500); //0.5秒待つ } void loop() { int current_position = previous_position; boolean reverse = digitalRead(in0) == LOW; if (digitalRead(in1) == LOW) { if (reverse) { current_position = -610; //停止位置1の反転位置設定 } else { current_position = 1190; //停止位置1の位置設定 } } if (digitalRead(in2) == LOW) { if (reverse) { current_position = -885; //停止位置2の反転位置設定 } else { current_position = 913; //停止位置2の位置設定 } } if (digitalRead(in3) == LOW) { if (reverse) { current_position = -400; //停止位置3の反転位置設定 } else { current_position = 1410; //停止位置3の位置設定 } } if (digitalRead(in4) == LOW) { if (reverse) { current_position = -540; //停止位置4の反転位置設定 } else { current_position = 1260; //停止位置4の位置設定 } } if (digitalRead(in5) == LOW) { if (reverse) { current_position = -328; //停止位置5の反転位置設定 } else { current_position = 1485; //停止位置5の位置設定 } } if (previous_position != current_position) { byte data[12]; int temp_position; Serial.write(torque_on,9); //サーボトルクON delay(100); //0.1秒待つ //今回の移動時間を計算 int move_speed = short(abs((long) previous_position - (long) current_position) * (long) rotate_speed / 3600); memcpy(data, position_data, 12); //回転データ転送 if (current_position < previous_position) { temp_position = current_position + reverse_offset; } else { temp_position = current_position + foward_offset; } memcpy(data, position_data, 12); //回転データ転送 memcpy(&data[7], &temp_position, 2); //変更位置設定 memcpy(&data[9], &move_speed, 2); //移動時間設定 checksum(data, 12); //チェックサム計算 Serial.write(data,12); //変更位置まで回転 delay(move_speed * 10 + 200); //回転終了待つ Serial.write(torque_off,9); //サーボトルクOFF previous_position = current_position; } delay(100); //スイッチチェック間隔