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

続き

お待ちかねのゲタが届きました。

これで5軸制御のテストが出来る様になります。

急いで基板を作って行く事にします。

でも、その前に

遊星ギヤBOXを使ってスカラロボット風に組んでみました。

動かしている様子

www.youtube.com

2軸での30分の耐久実験も行いましたが最後までズレることなく完走しています。

俄然、やる気が出ます。

プログラムの方も進化しました。

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;
boolean finish = false;

hw_timer_t * timer = NULL;
void IRAM_ATTR onTimer() {
if (!start)
return;
if (finish) {
start = false;
finish = false;
toggle_set = true;
toggle_get = false;
data_clear();
dir_clear();
pos = 0;
num = 0;
return;
}
if(toggle_set) { // 初回 true
if (step_data1[num] & 0x80)
setDirection(step_data1[num++]);
if (step_data1[num] & 0x40)
finish = true;
else
rotation1(num);
}
else {
if (step_data2[num] & 0x80)
setDirection(step_data2[num++]);
if (step_data2[num] & 0x40)
finish = true;
else
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 data_clear() {
for (int i = 0;i < 10000;i++) {
step_data1[i] = 0;
step_data2[i] = 0;
}
}

void dir_clear() {
for (int i = 0;i < 6;i++) {
digitalWrite(dirPins[i],0);
}
}

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],d & 0x40); // finishで利用
// 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の第6ビットを利用して終了信号を受け取る様に変更

今まではリセットボタンで初期化していましたが自動で初期化でき次々へと新しいデータを送ることが出来ます。

今後の展開

5軸制御基板が完成すれば様々な実験が出来る様になり一気に進むような予感

楽しみ!

また、更新します。