#include "Motion86.h"
#include "SD.h"
// マシンオブジェクトを生成します。マシン0~2まで、それぞれ3軸のマシンを最大3台まで使用できます。
// この例では、マシン0とマシン1を使用します。
Machine machine(0);
Machine slave_machine(1);
// ステッピングモーター enable pin。
int EnablePin = 4;
// この gcode ファイルは SD カードに保存されます。
char *filename = "auto.gcode" ;
File gfile;
char buf[256];
int ptr;
bool working;
void setup() {
while (!Serial);
pinMode(EnablePin, OUTPUT);
// 必要に応じて、軸の移動方向を反転できます。
// この例では、x 軸と y 軸の方向を反転する必要があります。
machine.config_ReverseDirection(AXIS_X);
machine.config_ReverseDirection(AXIS_Y);
// PPU (単位あたりのパルス) は、さまざまなニーズに応じた仮想的な長さの単位です。
// この例では、x 軸の単位長さは 80 パルスに設定されており、これは実際のアプリケーションでは 1 mm に相当します。
machine.config_PPU(AXIS_X, 80.0);
machine.config_PPU(AXIS_Y, 80.0);
machine.config_PPU(AXIS_Z, 1600.0);
// 機械の動きに対するソフトウェア制限を設定します。
machine.config_PosLimit(AXIS_X, 0, 300);
machine.config_PosLimit(AXIS_Y, 0, 200);
machine.config_PosLimit(AXIS_Z, 0, 300);
// リミットスイッチがホームポイントを設定するために使用するピンを設定します。
machine.config_HomePins(2, 7, 8);
// この場合、slave_machine は y 軸の方向を反転する必要があります。
slave_machine.config_ReverseDirection(AXIS_Y);
// スレーブマシンの PPU (単位あたりのパルス) を設定します。
slave_machine.config_PPU(AXIS_X, 80.0);
slave_machine.config_PPU(AXIS_Y, 80.0);
slave_machine.config_PPU(AXIS_Z, 3200.0);
// スレーブ マシンの動作に対するソフトウェア制限を設定します。
slave_machine.config_PosLimit(AXIS_X, 0, 200);
slave_machine.config_PosLimit(AXIS_Y, 0, 200);
slave_machine.config_PosLimit(AXIS_Z, 0, 300);
// ホームポイントのリミットスイッチを設定するために、slave_machine が使用するピンを設定します。
slave_machine.config_HomePins(21, 22, 23);
//マシンの EGearSlave にslave_machine を設定します。これは、slave_machine がマシンの動きを計算して追従することを意味します。
machine.config_EGearSlave(slave_machine, 1.5, 0.5, 1.0);
// 制御する前に、マシンを起動する必要があります。
machine.machineOn();
// ソフトウェア制限を有効にします。
machine.enableSoftLimit();
// ホームポイントへの送り速度を設定します。
machine.setHomeSpeed(1000, 1000, 200);
slave_machine.setHomeSpeed(1000, 1000, 100);
// SDカード内のGコードファイルを開く
if (SD.exists(filename)) {
gfile = SD.open(filename);
working = true ;
} else {
Serial.print( "File not exists: " );
Serial.println(filename);
while (1);
}
// ステッピングモーターを起動します。
digitalWrite(EnablePin, LOW);
// リミットスイッチによって定義されたホームポイントに戻ります。
machine.home();
}
void loop() {
// gcode ファイルを読み取って解析します。
// slave_machine は、幅 x1.5、高さ x0.5 のスケールされた gcode パスです。
if (working && !machine.isCmdBufferFull()) {
ptr = 0;
while (gfile.available()) {
buf[ptr] = gfile.read();
if (buf[ptr] == '\n' ) {
buf[ptr + 1] = '\0' ;
machine.gcode(buf);
break ;
} else {
ptr ++;
}
}
if (!gfile.available())
{
Serial.println( "GCODE FINISH" );
gfile.close();
working = false ;
}
}
}
|