スマホで動くロボットアームを作る

続き

前回の遊星ギヤで制御してみました。

また、プログラムもだいぶと進化しました。

今回のプログラム

byte stepPins = {15, 0, 5, 19, 22, 32, 25, 27};
byte dirPins = {2, 4, 18, 21, 23, 33, 26, 14};
byte step_data1[10000] = {0};
byte step_data2[10000] = {0};
byte inData = 0;
int pos = 0;
int num = 0;
boolean toggle_set = true;
boolean toggle_get = false;
boolean start = false;

hw_timer_t * timer = NULL;
void IRAM_ATTR onTimer() {
if (!start)
return;
if(toggle_set) { // 初回 true
if (step_data1[num] & 0x80)
setDirection(step_data1[num++]);
rotation1(num);
}
else {
if (step_data2[num] & 0x80)
setDirection(step_data2[num++]);
rotation2(num);
}
num++;
if (num == 10000) {
num = 0;
toggle_set = !toggle_set;
}
}

void setup() {
for (int i = 0; i < sizeof(stepPins); i++) {
pinMode(stepPins[i], OUTPUT);
}
for (int i = 0; i < sizeof(dirPins); i++) {
pinMode(dirPins[i], OUTPUT);
}
Serial.begin(115200);
timer = timerBegin(0, 80, true);
timerAttachInterrupt(timer, &onTimer, true);
timerAlarmWrite(timer, 100, true);
timerAlarmEnable(timer);
}

void loop() {
if (Serial.available() > 0) {
inData = Serial.read();
if(toggle_get) // 初回 false
step_data2[pos++] = inData;
else
step_data1[pos++] = inData;
if (pos == 10000) {
pos = 0;
toggle_get = !toggle_get;
start = true;
}
}
}

void setDirection(byte d) {
digitalWrite(dirPins[0],digitalRead(dirPins[0]) ^ (d & 0x01)); // ビットが立っていたら反転
digitalWrite(dirPins[1],digitalRead(dirPins[1]) ^ (d & 0x02) >> 1); // ビットが立っていたら反転
digitalWrite(dirPins[2],digitalRead(dirPins[2]) ^ (d & 0x04) >> 2); // ビットが立っていたら反転
digitalWrite(dirPins[3],digitalRead(dirPins[3]) ^ (d & 0x08) >> 3); // ビットが立っていたら反転
digitalWrite(dirPins[4],digitalRead(dirPins[4]) ^ (d & 0x10) >> 4); // ビットが立っていたら反転
digitalWrite(dirPins[5],digitalRead(dirPins[5]) ^ (d & 0x20) >> 5); // ビットが立っていたら反転
digitalWrite(dirPins[6],digitalRead(dirPins[6]) ^ (d & 0x40) >> 6); // ビットが立っていたら反転

// digitalWrite(dirPins[7],d & 0x80); // setDirectionで利用
}

void rotation1(int num) {
digitalWrite(stepPins[0],step_data1[num] & 0x01);
digitalWrite(stepPins[1],step_data1[num] & 0x02);
digitalWrite(stepPins[2],step_data1[num] & 0x04);
digitalWrite(stepPins[3],step_data1[num] & 0x08);
digitalWrite(stepPins[4],step_data1[num] & 0x10);
digitalWrite(stepPins[5],step_data1[num] & 0x20);
digitalWrite(stepPins[6],step_data1[num] & 0x40);
// digitalWrite(stepPins[7],step_data1[num] & 0x80);
}

void rotation2(int num) {
digitalWrite(stepPins[0],step_data2[num] & 0x01);
digitalWrite(stepPins[1],step_data2[num] & 0x02);
digitalWrite(stepPins[2],step_data2[num] & 0x04);
digitalWrite(stepPins[3],step_data2[num] & 0x08);
digitalWrite(stepPins[4],step_data2[num] & 0x10);
digitalWrite(stepPins[5],step_data2[num] & 0x20);
digitalWrite(stepPins[6],step_data2[num] & 0x40);
// digitalWrite(stepPins[7],step_data2[num] & 0x80);
}

変更点

回転方向の指示はstep_dataの第7ビットを利用。これにより最高7軸の制御になりますが、どのタイミングでも反転信号を検知出来ます。

このプログラムで動かした様子

www.youtube.com