/* 歯切りマシンの制御ソフト 20221007_GearCutter.ino 2022/10/07 ラジオペンチ http://radiopench.blog96.fc2.com/ */ #include #define input1 4 // input1 @ L298N #define input2 5 // 2 #define input3 7 // 3 #define input4 8 // 4 #define enableA 9 // enableA #define enableB 10 // enableB #define stepSW 11 // startスイッチ (LOWで運転) #define autoSW 12 // startスイッチ (LOWで運転) #define LOW_POSIT 100 // 砥石下端位置 角度 #define UP_POSIT 52 // 砥石上昇位置 #define NGT 9.0 // 歯の数(Number of Gear Teeth)浮動小数点で指定 unsigned int pos = 32000; // 回転位置記録用変数 int t = 1500; // パルスレート設定 単位:us int revCount = 0; float absPosi = 0.0; float k1 = (19.0 * 29.0) / (200.0 * 94.0 * 83.0); // 1パルス当たりの回転量(1回転=1.0) float k2 = 1.0 / NGT; // 1分割の回転量 float posit = 0.0; Servo myservo; void setup() { Serial.begin(115200); myservo.attach(3); // サーボモーターをPin3で使う pinMode(13, OUTPUT); pinMode(autoSW, INPUT_PULLUP); // Autoスイッチ SW-1(LowでAuto) pinMode(input1, OUTPUT); pinMode(input2, OUTPUT); pinMode(enableA, OUTPUT); pinMode(input4, OUTPUT); pinMode(input3, OUTPUT); pinMode(enableB, OUTPUT); pinMode(stepSW, INPUT_PULLUP); // 連続回転スイッチ SW-2 digitalWrite(enableA, HIGH); // 出力許可 digitalWrite(enableB, HIGH); if (digitalRead(stepSW) == LOW) { // SW-2が押されたままリセットされていたら while (1) { ccwP(); // 永久に回転(軸の偏心チェック等に使用) } } Serial.println(); myservo.write(UP_POSIT); // 砥石をUPポジションで待機 delay(200); } void loop() { while (digitalRead(autoSW) == HIGH) { // Autoスイッチが押されていなければ待機 } diskDown(); // 砥石をゆっくり下げる(加工位置へ移動) delay(1500); // 研削加工 diskUp(); // 砥石上昇 posit = posit + k2; // インデックス値更新(割り切れない値なので浮動小数点で指定) Serial.print(posit, 8); // 送り量出力 moveTo(posit); // 1ステップ送り delay(100); } void diskUp() { // 砥石上昇 myservo.write(UP_POSIT); delay(200); } void diskDown() { // 砥石をゆっくり降ろす for (int i = UP_POSIT; i < LOW_POSIT; i++) { myservo.write(i); delay(100); } } void moveTo(float newAngle) { // 指定角度に移動 static int32_t lastPosit = 0; // 現在のモーター位置 int32_t newPosit; int16_t drivePulse; newPosit = newAngle / k1; drivePulse = newPosit - lastPosit; // 駆動パルス数計算 driveCCW(drivePulse); Serial.print(", "); Serial.println(drivePulse); lastPosit = newPosit; } void driveCCW(int p) { for (int i = 0; i < p; i++) { ccwP(); } } void ccwMove(float x) { int d; d = 2831.942 * x; Serial.println(d); for (int i = 0; i < d; i++) { ccwP(); } } void cwP() { // CW方向へ1パルス pos++; PMdrive(pos); } void ccwP() { // CCW方向へ1パルス pos--; PMdrive(pos); } void PMdrive(unsigned int n) { // 指定状態にコイルを励磁 int phase; digitalWrite(13, HIGH); phase = n % 4; // 位相を求め switch (phase) { // 相当する状態に通電 case 0: digitalWrite(input1, HIGH); // A+ digitalWrite(input2, LOW); digitalWrite(input3, HIGH); // B+ digitalWrite(input4, LOW); break; case 1: digitalWrite(input1, LOW); digitalWrite(input2, HIGH); // A- digitalWrite(input3, HIGH); // B+ digitalWrite(input4, LOW); break; case 2: digitalWrite(input1, LOW); digitalWrite(input2, HIGH); // A- digitalWrite(input3, LOW); digitalWrite(input4, HIGH); // B- break; case 3: digitalWrite(input1, HIGH); // A+ digitalWrite(input2, LOW); digitalWrite(input3, LOW); digitalWrite(input4, HIGH); // B- break; } delayMicroseconds(t); // 一定時間待つ digitalWrite(13, LOW); }