スティックフォーカーサー Part 2

Tools

昨夜実戦投入して、EkosのFocus Moduleで完璧に動作しました。北海道ではピント合わせも凍えてしまいますので、すべてをオートマチックにしたい。AZ-GTiの経緯台モードなら鏡筒を水平にして大雑把に北に向けて、あとは遠隔。最高。赤道儀モードだと最初に極軸合わせがあるから、それをどれだけ短時間で済ますかが鍵。凍え死にます。

さて、前回スティックフォーカーサーを完成させました。前回使った温度・湿度センサーDHT11は、旧タイプで手に入りづらいので、新タイプのものでケースを作り直しました。

これはRed Cat51用です。

鏡筒へのセットは、3Dプリンターでアームを作りました。

円形のホールドに差し込むだけ、タイミングベルト(280mm)のテンションの調整は回転させるだけ。前後の調整も前後にスライドさせるだけ。超便利。

3Dプリンタ用データはこちら

タイミングベルトは、Amazonで注文しました。しかし中国発送のタイミングベルトは届きませんでした。結局MISUMI NOVAで注文、中国の値段とは比較にならないけど、品質と3日程度で届くの安心です。Red Cat51等のヘリコイド(ギザギザのピント合わせのリング)は、測るとピッチが3mm。3mmのタイミングベルトは手に入るけど、ピッチ3mmのプーリーの値段が高い。2mmのタイミングベルトで試したけど問題なくヘリコイドが回ったので安価に手に入る2mmピッチで行きます。

商品型番280-2GT-6
メーカー名ゲイツ・ユニッタ・アジア
商品名称パワーグリップGTベルト 2GT
コンパクトで良いね!

その後、コードを解析して使い方を大まかに把握しました。温度補正についてはINDIの方でどこまでやってくれるのかコードを見ただけでは分からなかったので、今後使いながら理解していこうと思います。自分が必要な部分のコメントを日本語化しました。末尾に掲載。

【インストール方法】

  1. PC(Win,Mac,Linux,Raspberry Pi)にArduino IDEをインストール
  2. ここではRaspberry PiにArduino IDE(Arduinoの開発環境アプリ)をインストール
    1. ターミナルでsudo apt-get install arduino [enter] ←これが一番簡単ですが超古いバージョンがインストールされるのでこの後進めなくなります。
    2. ブラウザでArduinoサイトからArduino 1.8.16のLinux ARM32bits版をダウンロード「1.8.xxの部分がアップデートされていたら次行以降も数字を合わせる。(ダウンロードフォルダにダウンロードされているはず)
    3. ターミナルで sudo tar -xzf ~/Downloads/arduino-1.8.16-linuxarm.tar.xz –directory /tmp/ [enter]
    4. ターミナルで sh /tmp/arduino-1.8.16/install.sh [enter] これでインストール完了。メニューのEducation(教育)の中にArduino IDEがあります。デスクトップにもショートカットが。
    5. Arduino IDEを起動。必要なライブラリ(Arduinoと接続するセンサーやモーターとの橋渡しをするプログラム)を追加。ここではステッパーモーターを動かすAccelStepperと温度・湿度センサーを動かすDHT sensor libraryを追加します。
    6. Sketchメニュー > Include Library > Manage Librariesから、フィルタでAccelStepperを検索します。(Arduinoは検索が遅いので焦らず表示されるのを待ちます。)表示されたら、インストールボタンを押してインストール。同じくDHT sensor libraryも検索してインストール。途中ダイアログが表示されるのでinstall Allを選択。
  3. NanoをUSBケーブルでRaspberry Piに接続。
  4. ツールメニュー > ボード > (Arduino AVR Boards >) Arduino Nano を選択。
  5. ツールメニュー > プロセッサ > ATmega328P を選択。
  6. ツールメニュー > シリアルポート > Nanoが接続されたUSBポートを選択。分からなければNanoを抜き差しして消えたり増えたりするのがそれ!
  7. 新規ファイルを開き中身を全部消します。下記のコードをコピペして、任意の名前を付けて保存します。
  8. ☑️ボタン(検証:コンパイル)を押してコードチェック。
  9. ➡️ボタンを押してコードをNanoへ書き込みます。

配線は図の通りです。

  • Arduino D2 >>> ULN2003 IN1
  • D3 >>> IN2
  • D4 >>> IN3
  • D4 >>> IN4
  • 5V >>> ( + )
  • GND >>> ( – )
  • 3V3 >>> DHT11 Vcc
  • GND >>> GND
  • D10 >>> DATA

※Nano は複数のチップが存在しNanoがUSBポートに認識されない場合は、PCの方にドライバのインストールが必要です。Macは必要でした。Raspberry Piには不要でした。ダウンロードは例えばこことか

後は断線しないようにケースに押し込みます。ステッパーモーターのコードが長いので苦戦します。

INDIに接続する前に動作確認をする場合は、上記7番からの手順で下記のコードを書き込みます。単純に90度を行ったり来たりするプログラムです。動作に問題がなければ、本コードを改めて書き込みます。これでINDIでフォーカーサーとして認識され動作します。フォーカーサードライバはMoonliteを選択します。

【ステッパーモータ動作確認コード】

#include <Stepper.h>

const int numSteps = 32;
const int oneRotation = 32 * 64; // 2048
const int CW = 1;
const int CCW = -1;

// initialize the stepper library on pins 0 through 3:
Stepper stepper(numSteps, 2, 4, 3, 5); // :0-2,1-3 piar

void setup() {
  // set the speed at 300 rpm:
  stepper.setSpeed(300);
  Serial.begin(9600);
}

void loop() {
  int r=oneRotation/4; // 90 degrees
  Serial.println("clockwise");
  stepper.step(CW*r);
  delay(2000);

  Serial.println("counterclockwise");
  stepper.step(CCW*r);
  delay(2000);
}

【DHT11 温度&湿度センサー動作確認コード】

ツールメニュー > シリアルモニタ(通信速度9600)で温度と湿度が2秒毎に表示されればオーケー。

// Example testing sketch for various DHT humidity/temperature sensors
// Written by ladyada, public domain

// REQUIRES the following Arduino libraries:
// - DHT Sensor Library: https://github.com/adafruit/DHT-sensor-library
// - Adafruit Unified Sensor Lib: https://github.com/adafruit/Adafruit_Sensor

#include "DHT.h"

#define DHTPIN 10     // Digital pin connected to the DHT sensor
// Pin 15 can work but DHT must be disconnected during program upload.

#define DHTTYPE DHT11   // DHT 11

DHT dht(DHTPIN, DHTTYPE);

void setup() {
  Serial.begin(9600);
  Serial.println(F("DHTxx test!"));

  dht.begin();
}

void loop() {
  // Wait a few seconds between measurements.
  delay(2000);

  // Reading temperature or humidity takes about 250 milliseconds!
  // Sensor readings may also be up to 2 seconds 'old' (its a very slow sensor)
  float h = dht.readHumidity();
  // Read temperature as Celsius (the default)
  float t = dht.readTemperature();
  // Read temperature as Fahrenheit (isFahrenheit = true)
  float f = dht.readTemperature(true);

  // Check if any reads failed and exit early (to try again).
  if (isnan(h) || isnan(t) || isnan(f)) {
    Serial.println(F("Failed to read from DHT sensor!"));
    return;
  }

  // Compute heat index in Fahrenheit (the default)
  float hif = dht.computeHeatIndex(f, h);
  // Compute heat index in Celsius (isFahreheit = false)
  float hic = dht.computeHeatIndex(t, h, false);

  Serial.print(F("Humidity: "));
  Serial.print(h);
  Serial.print(F("%  Temperature: "));
  Serial.print(t);
  Serial.print(F("°C "));
  Serial.print(f);
  Serial.print(F("°F  Heat index: "));
  Serial.print(hic);
  Serial.print(F("°C "));
  Serial.print(hif);
  Serial.println(F("°F"));
}

【Moonlite互換フォーカスコントローラコード】

// Moonlite-compatible stepper controller
//
// Uses AccelStepper (http://www.airspayce.com/mikem/arduino/AccelStepper/)
//1 2 3 4 5 6 7 8   返り値       説明
//: D #             N/A         デバックモード オン/オフ切り替え(Arduino IDEのシリアルモニタでD#を送信)
//: C #             N/A         温度の変換を開始します。変換処理には最大750ミリ秒かかります。 :GT#コマンドで返される値は、変換処理が完了するまで有効ではありません。
//: F G #           N/A         SNYYYY#コマンドで設定された新しい位置に移動します。
//: F Q #           N/A         フォーカスモーターの動きを直ちに停止します。
//: G C #           XX#         温度係数を返します。XXは2桁の符号付き(2の補数)16進数です。
//: G D #           XX#         XXは2桁の符号なし16進数で、現在のステッピングディレイを返す。可能な戻り値のリストは、:SD#コマンドを参照してください。
//: G H #           00# OR FF#  フォーカスモーターがハーフステップの場合は "FF#"、そうでない場合は "00#"を返します。
//: G I #           00# OR 01#  フォーカスモーターが動いていない場合は "00#"を、そうでない場合は "01#"を返します。
//: G N #           YYYY#       SNYYYY#コマンドで設定された新しい位置を返します(YYYYは4桁の符号なし16進数)。
//: G P #           YYYY#       YYYYを4桁の16進数で表した現在の位置を返す。
//: G T #           YYYY#       YYYY を 4 桁の符号付き(2 の補数)16 進数で表し、現在の温度を返す。
//: G V #           DD#         ファームウェアのバージョンを2桁の10進数で取得します。1桁目はメジャーバージョン、2桁目はマイナーバージョンです。
//: S C X X #       N/A         新しい温度係数を設定します。XXは2桁の符号付き(2の補数)16進数です。
//: S D X X #       N/A         新しいステップディレイを設定します。XXは2桁の符号なし16進数です。送信可能な値は02、04、08、10、20で、これはそれぞれ250、125、63、32、16ステップ/秒のステッピングディレイに対応する。
//: S F #           N/A         フルステップモードに設定。(機能しない)
//: S H #           N/A         ハーフステップモードに設定。(機能しない)
//: S N Y Y Y Y #   N/A         新しい位置設定します。YYYYは任意の4桁の数字
//: S P Y Y Y Y #   N/A         現在の位置を設定します。YYYYは任意の4桁の数字
//: + #             N/A         温度補正フォーカシングを有効にします。
//: - #             N/A         温度補正フォーカシングを無効にします。
//: P O X X #       N/A         温度校正オフセット、XX は 2桁の符号付き 16 進数で、半度単位で表示される。
//: Y M #           N/A         温度測定値の拡張(0.125度)
//: Y B X X #       N/A         バックラッシュの設定、XXは2桁の符号付き16進数です。
//: Z B #           XX#         バックラッシュの取得
//: Y T Y Y Y Y #   N/A         最大ステップ数の設定(YYYYは4桁の符号なし16進数)。
//: Z T #           YYYY#       最大ステップ数の取得
//: Y X X X #       N/A         TempCompしきい値の設定 XXは2桁の符号なし16進数で、0.25度の単位である。
//: Z X #           XX#         TempCompしきい値の取得
//: Y + #           N/A         温度補正フォーカシングを有効にします。
//: Y - #           N/A         温度補正フォーカシングを無効にします。
//: Z + #           00 or 01#   温度補正値を取得。
//: Z A #           YYYY#       平均温度を返す * 100 YYYYは4桁の符号付き(2の補数)16進数です。

//例 1: :PO02# offset of +1°C
//例 2: :POFB# offset of -2.5°C

#include <AccelStepper.h>
#include <DHT_U.h>
#include <EEPROM.h>

// Speed per "SD" unit
#define                SPEEDMULT 30                        // base unit of stepper speed

#define                TEMPSENSOR_ARRAY_SIZE 30            // array to track temperature history and to scalculate average temperatures
#define                TEMPERATURE_DEFAULT 25              // default temperature
#define                HUMIDITY_DEFAULT 25                 // default humidity

#define                TEMPCOMP_THRESHOLD 1                // Temperature change threshold to trigger TempComp movement since last TempComp
#define                TEMPCOMP_HYSTERESIS 1               // Hysteresis to report error, without moving focuser ???
#define                TEMPCOMP_MOVEDELAY 2000             // DELAY between 2 steps druing TempComp move
#define                STEPPER_DISABLEDELAY 5000           // DELAY to disable output driver after last move
#define                TEMPSENSOR_READDELAY 5000           // Temperature sensor read interval if driver does not poll
#define                TEMPSENSOR_SAMPLEDELAY 5000         // Temperature sample interval to calculate average temperature. For 30 samples at 5s interval will average out temperature in last 150s.

#define                PIN_OUTPUT_MOTOR1 2                 // Motor pins
#define                PIN_OUTPUT_MOTOR2 3
#define                PIN_OUTPUT_MOTOR3 4
#define                PIN_OUTPUT_MOTOR4 5
#define                PIN_INPUT_SENSOR 10                 // Tempeature sensors
#define                PIN_INPUT_BUT_FW 11                  // Maunal movement button
#define                PIN_INPUT_BUT_BW 12
#define                PIN_OUTPUT_STATUS 13                // To report error when temperature has gone up over hysteresis threshold when TempComp is on.

#define                LEDBLINK_INTERVAL   250             // 250ms
#define                LEDBLINK_CYCLE      16              // 16*250 = 4s
///////////////////////////
// Stepper
///////////////////////////

// ULN2003 requires IN1-IN3-IN2-IN4
AccelStepper           stepper(AccelStepper::FULL4WIRE, PIN_OUTPUT_MOTOR1, PIN_OUTPUT_MOTOR3, PIN_OUTPUT_MOTOR2, PIN_OUTPUT_MOTOR4, false);

///////////////////////////
// Temperature Sensor
///////////////////////////

DHT_Unified TempSensor(PIN_INPUT_SENSOR, DHT22);

///////////////////////////
// Temperature Signals
///////////////////////////

boolean                TempSensor_Present = false;                              // DHT22 present
float                  TempSensor_Reading = TEMPERATURE_DEFAULT;                // temperature reading from sensor
int16_t                TempSensor_Raw = 0;                                      // Raw temperature returned to the driver
float                  HumiSensor_Reading = HUMIDITY_DEFAULT;

///////////////////////////
// Serial Interface Signals
///////////////////////////

#define MAXCOMMAND 8
char                   inChar;
char                   cmd[MAXCOMMAND];
char                   param[MAXCOMMAND];
char                   packet[MAXCOMMAND];
boolean                eoc = false;
int                    idx = 0;

///////////////////////////
// Motor Control Signals
///////////////////////////

long                   TargetPosition = 0;
long                   CurrentPosition = 0;
boolean                isRunning = false;
// max/min limit when moving focuser manually.
// Max can be set via serial command YX.
long                   MaxSteps = 25000;
long                   MinSteps = 0;

///////////////////////////
// Speed multipler
///////////////////////////

// multiplier of SPEEDMUX, currently max speed is 480.
int                    SpeedFactor = 16;
int                    SpeedFactorRaw = 2;

///////////////////////////
// Temperature Compensation
///////////////////////////

// TemoComp coefficient is signed integer
int                    TempCoefficientRaw = 1;
int                    TempCoefficient = 1;

// TemmpComp temperature drop threshold to trigger TempComp.
// NOW temperature increase does not trigger TempComp, instead it will be reported as ERROR.
float                  TempCompThreshold = TEMPCOMP_THRESHOLD;
int                    TempCompThresholdRaw = 0;

boolean                TempCompEn = false;

boolean                TempCompError = false;

// TempComp original position and temeprature.
// this is to avoid losing steps, eg Coefficient*Threshold < 1, so it will not move if we only keep track of the different between 2 "regions".
// so we need to use the original temperature and position to calculate the "supposed to be" target position.
float                  TempCompOriginalTemperature = TEMPERATURE_DEFAULT;
long                   TempCompOriginalPosition = 0;
long                   TempCompTargetPosition = 0;
float                  TempCompLastTemperature = TEMPERATURE_DEFAULT;

float                  TempSensor_Array[TEMPSENSOR_ARRAY_SIZE];
float                  TempSensor_Array_Total = 0;
float                  TempSensor_Average = TEMPERATURE_DEFAULT;
boolean                TempSensor_Valid_Array[TEMPSENSOR_ARRAY_SIZE];
int                    TempSensor_Valid_Total;

// backlash管理のために下記の2つから選択する。選択しない方をコメントアウト(//を前に追加)

// #define SIMPLE_BACKLASH
#define OUTWARD_BACKLASH

// Simple backlash management
// - When motor step was decreasing, and is now increasing, apply a positive backlash
// - When motor step was increasing, and is now decreasing, apply a negative backlash
// This causes the firmware to return P+/-backlash as position when requested to go to position P
// - モータのステップが減少方向から増加方向に転じた場合、正のバックラッシュを適用する
// - モータのステップが増加方向から減少方向に転じた場合、負のバックラッシュを適用する
// これにより、P位置への移動を要求された場合、ファームウェアはP +/- バックラッシュを位置として返すようになります。

// Outward backlash management (idea by Richard Beck on indilib.org)
// - When motor step is requested to increase, add a positive backlash, then when move is finished, move backwards by the same backlash
// - When motor step is requested to decrease, move to the requested position
// This causes the firmware to return P as position when requested to go to position P, and makes sure gear backlash is always outward, preventing slipping
// 外側向きのみバックラッシュ管理 (indilib.orgのRichard Beck氏によるアイデア)
// - モータのステップアップ時に正のバックラッシュを加え、動作終了時に同じバックラッシュで後退させる。
// - モーターステップダウンが要求された場合、要求された位置に移動する
// これにより、P位置への移動を要求された場合、ファームウェアはP位置を返すようになり、ギアのバックラッシュが常に外側になるようになり、スリップを防ぐことができます。

#define WILL_GO_INWARDS(current_pos, next_pos) ((current_pos) < (next_pos))
#define WILL_GO_OUTWARDS(current_pos, next_pos) ((current_pos) > (next_pos))
#define INWARDS_BY(pos, offset) ((pos)+(offset))
#define OUTWARDS_BY(pos, offset) ((pos)-(offset))

// Backlash to be used on next change of direction - long for eeprom
long                   Backlash = 0; // [FPTN,FNTP]
#define                BACKLASH_FNTP (+11)
#define                BACKLASH_FPTN (-11)

//#define                DIRUP false
//#define                DIRDOWN true

//bool                   TempCompLastDir = DIRDOWN;
///////////////////////////
// Timer
///////////////////////////

unsigned long                   millisLastMove = 0;                // Last move timer to turn off stepper output
unsigned long                   millisLastTempSensorLatch = 0;     // Last temperature sample timer
unsigned long                   millisLastTempSensorRead = 0;      // Last temperature sensor read timer
unsigned long                   millisLastTempCompMove = 0;        // Last move timer during TempComp

///////////////////////////
//Manual move control
///////////////////////////

#define                BUT_MOVEMENT_ENABLED 0
#define                BUT_READING_RELEASED 0
#define                BUT_READING_PRESSED 1

//int                    lastReadingButFW = BUT_READING_RELEASED;               //
//int                    lastReadingButBW = BUT_READING_RELEASED;

// Button press timer to increase motor move steps (ie, effective motor speed).
unsigned long                   millisButFWPressed = 0;
unsigned long                   millisButBWPressed = 0;

///////////////////////////
// EEPROM interface
///////////////////////////

#define                EEPROM_POS_LOC 0
long                   lastSavedPosition = 0;
#define                EEPROM_POS_BACKLASH 8

///////////////////////////
// LED signals
///////////////////////////

unsigned long          millisLastLEDBlink = 0;
int                    blinkTimer = 0;

///////////////////////////
// Misc signals
///////////////////////////

// Moonlite compatability mode - 0.5 degree temparture reading accuracy
// Set to false will return 0.125 accuracy
boolean                MoonliteMode = true;

int                    i;

bool debug = true;
//bool debug = false;

void setup()
{
  Serial.begin(9600);

  pinMode (PIN_INPUT_SENSOR, INPUT);
  //pinMode (PIN_INPUT_BUT_FW, INPUT_PULLUP);
  //pinMode (PIN_INPUT_BUT_BW, INPUT_PULLUP);
  pinMode (PIN_OUTPUT_STATUS, OUTPUT);

  // Initialize temperature array
  for (i = 0; i < TEMPSENSOR_ARRAY_SIZE; i++)
  {
    TempSensor_Array[i] = TEMPERATURE_DEFAULT;
    TempSensor_Valid_Array[i] = false;
  }

  // Initialize DHT22 - test temperature readout
  TempSensor.begin();
  sensors_event_t t_event;
  TempSensor.temperature().getEvent(&t_event);
  TempSensor_Reading = t_event.temperature;
  TempSensor_Present = !isnan(TempSensor_Reading);

  if (TempSensor_Present and debug)
  {
    sensor_t sensor;
    {
      TempSensor.temperature().getSensor(&sensor);
      /*
        Serial.println("------------------------------------");
        Serial.println("Temperature");
        Serial.print ("Sensor: "); Serial.println(sensor.name);
        Serial.print ("Driver Ver: "); Serial.println(sensor.version);
        Serial.print ("Unique ID: "); Serial.println(sensor.sensor_id);
        Serial.print ("Max Value: "); Serial.print(sensor.max_value); Serial.println(" *C");
        Serial.print ("Min Value: "); Serial.print(sensor.min_value); Serial.println(" *C");
        Serial.print ("Resolution: "); Serial.print(sensor.resolution); Serial.println(" *C");
        unsigned long ticks = millis();
        float v = DHT_getTemperature();
        ticks = millis() - ticks;
        Serial.print ("Temperature: "); Serial.print(v); Serial.print(" *C ("); Serial.print(ticks); Serial.println(" ms)");
        Serial.println("------------------------------------");
      */
    }
    {
      TempSensor.humidity().getSensor(&sensor);
      /*
        Serial.println("Humidity");
        Serial.print ("Sensor: "); Serial.println(sensor.name);
        Serial.print ("Driver Ver: "); Serial.println(sensor.version);
        Serial.print ("Unique ID: "); Serial.println(sensor.sensor_id);
        Serial.print ("Max Value: "); Serial.print(sensor.max_value); Serial.println(" %");
        Serial.print ("Min Value: "); Serial.print(sensor.min_value); Serial.println(" %");
        Serial.print ("Resolution: "); Serial.print(sensor.resolution); Serial.println(" %");
        unsigned long ticks = millis();
        float v = DHT_getHumidity();
        ticks = millis() - ticks;
        Serial.print ("Humidity: "); Serial.print(v); Serial.print(" % ("); Serial.print(ticks); Serial.println(" ms)");
        Serial.println("------------------------------------");
      */
    }
  }

  millisLastTempSensorRead = millis();
  millisLastTempSensorLatch = millis();

  // initalize motor
  stepper.setMaxSpeed(SpeedFactor * SPEEDMULT);
  stepper.setAcceleration(100);
  millisLastMove = millis();

  // initialize serial command
  memset(packet, 0, MAXCOMMAND);

  // read saved position from EEPROM
  EEPROM.get(EEPROM_POS_LOC, CurrentPosition);
  stepper.setCurrentPosition(CurrentPosition);
  lastSavedPosition = CurrentPosition;
  EEPROM.get(EEPROM_POS_BACKLASH, Backlash);
  if (Backlash != BACKLASH_FNTP || Backlash != BACKLASH_FPTN || Backlash != 0)
    Backlash = 0;

  if (debug)
  {
    Serial.print("Focuser current position: "); Serial.print(CurrentPosition); Serial.println("");
#if defined SIMPLE_BACKLASH
    Serial.println("Simple backlash management");
#elif defined OUTWARD_BACKLASH
    Serial.println("Outward backlash management");
#else
    Serial.println("No backlash management");
#endif
  }
}

void loop()
{
  double    Scratch_Double;
  int       Error_Code;

  char tempString[32];
  memset(tempString, '\0', sizeof(tempString));

  if (eoc) {
    // process the command we got
    cmd[0] = cmd[1] = cmd[2] = '\0';
    memset(param, 0, MAXCOMMAND);

    int len = strlen(packet);

    if (packet[0] == 'C' || packet[0] == '+' || packet[0] == '-')
    {
      cmd[0] = packet[0];
    }
    else
    {
      cmd[0] = packet[0];
      cmd[1] = packet[1];
      if (len > 2)
        strncpy(param, packet + 2, len - 2);
    }

    packet[0] = '\0';
    eoc = false;
    idx = 0;

    if (debug)
    {
      snprintf(tempString, sizeof(tempString), "Cmd: %.2s - Arg: %.6s", cmd, param);
      Serial.println("");
      Serial.println(tempString);
    }

    // the stand-alone program sends :C# :GB# on startup
    // :C# is a temperature conversion, doesn't require any response

    // initiate temperature conversion
    if (!strcasecmp(cmd, "C")) {
      // do nothing
      //if (TempSensor_Present) {
      //  TempSensor.requestTemperatures();
      //}
    }

    // toggle debug on/off
    // デバッグモード切り替え
    if (!strcasecmp(cmd, "D")) {
      debug = !debug;
      if (debug)
      {
        Serial.println("Debug enabled\n");
      }
      else
      {
        Serial.println("Debug disabled\n");
      }
    }

    // initiate a move
    if (!strcasecmp(cmd, "FG")) {
      // Ignore move when Temp Comp is enabled
      // Need to revisit as there could be MOVE due to filter change
      if (!TempCompEn)
      {
        CurrentPosition = stepper.currentPosition();
        stepper.enableOutputs();

        /*if(debug)
          {
          snprintf(tempString, sizeof(tempString), "C%04X T%04X B%+02d",(int)CurrentPosition,(int)TargetPosition,(int)Backlash);
          Serial.println(tempString);
          }*/

#if defined(SIMPLE_BACKLASH)
        // If switching direction, add backlash to compensate for the next move
        if ((CurrentPosition < TargetPosition && 0 < Backlash) || (TargetPosition < CurrentPosition && Backlash < 0))
          TargetPosition += Backlash;
        // Then, prepare backlash for the next move
        if (CurrentPosition < TargetPosition)
          Backlash = BACKLASH_FPTN;
        else if (TargetPosition < CurrentPosition)
          Backlash = BACKLASH_FNTP;
        else
          Backlash = 0;
#elif defined(OUTWARD_BACKLASH)
        // If going inwards, offset requested position by backlash, and plan for additional backlash that will be applied outwards
        // If going outwards, use requested position unmodified, backlash is assumed handled by a prior move
        Backlash = WILL_GO_INWARDS(CurrentPosition, TargetPosition) ? 2 * BACKLASH_FNTP : 0;
        TargetPosition = INWARDS_BY(TargetPosition, Backlash);
#endif

        if (debug)
        {
          snprintf(tempString, sizeof(tempString), "FG: C%04X T%04X B%+02d", (int)CurrentPosition, (int)TargetPosition, (int)Backlash);
          Serial.println(tempString);
        }

        stepper.moveTo(TargetPosition);

        if (debug) outputDebugState('>');
      }
    }

    // stop a move
    // stepper.stop() stops motor gracefully, as a result motor may continue running for sometime (upto 1000 step at max speed setting), depending the current speed.
    // if we stop the motor abruptly then somehow stepper library does not handle current/target position correctly.
    if (!strcasecmp(cmd, "FQ")) {
      // FIXME: manage backlash
      stepper.stop();
    }

    // get the temperature coefficient which is set by SC
    if (!strcasecmp(cmd, "GC")) {
      //char tempString[6];
      sprintf(tempString, "%02X", TempCoefficientRaw);
      Serial.print(tempString);
      Serial.print("#");
    }

    // get the current motor speed, only values of 02, 04, 08, 10, 20, which is set by SD
    if (!strcasecmp(cmd, "GD")) {
      //char tempString[6];
      sprintf(tempString, "%02X", SpeedFactorRaw);
      Serial.print(tempString);
      Serial.print("#");
    }

    // whether half-step is enabled or not, always return "00"
    if (!strcasecmp(cmd, "GH")) {
      Serial.print("00#");
    }

    // motor is moving - 01 if moving, 00 otherwise
    if (!strcasecmp(cmd, "GI")) {
      if (isRunning) {
        Serial.print("01#");
      }
      else {
        Serial.print("00#");
      }
    }

    // OUT-OF-SPEC get humidity
    if (!strcasecmp(cmd, "GM")) {
      // Skip humidity reading when motor is running
      if (stepper.distanceToGo() == 0) {
        if (TempSensor_Present)
          HumiSensor_Reading = DHT_getHumidity();
      }

      // reset temp sensor read timer.
      millisLastTempSensorRead = millis();

      //char tempString[6];
      if (MoonliteMode)
        // compatability mode, 0.5 percent resolution
        sprintf(tempString, "%04X", (int)(TempSensor_Reading / 0.5));
      // else 0.125 percent resolution
      else sprintf(tempString, "%04X", (int)(TempSensor_Reading / 0.125));
      Serial.print(tempString);
      Serial.print("#");
    }

    // get the new motor position (target) set by SN
    if (!strcasecmp(cmd, "GN")) {
      //char tempString[6];
      sprintf(tempString, "%04X", TargetPosition);
      Serial.print(tempString);
      Serial.print("#");
    }

    // get the current motor position
    if (!strcasecmp(cmd, "GP")) {
      CurrentPosition = stepper.currentPosition();
      //char tempString[6];
      sprintf(tempString, "%04X", CurrentPosition);
      Serial.print(tempString);
      Serial.print("#");
    }

    // get temperature
    if (!strcasecmp(cmd, "GT")) {
      // Skip temperature reading when motor is running
      if (stepper.distanceToGo() == 0) {
        if (TempSensor_Present)
          TempSensor_Reading = DHT_getTemperature();
      }

      // reset temp sensor read timer.
      millisLastTempSensorRead = millis();

      //char tempString[6];
      if (MoonliteMode)
        // compatability mode, 0.5 degeee resolution
        sprintf(tempString, "%04X", (int)(TempSensor_Reading / 0.5));
      // else 0.125 degree resolution
      else sprintf(tempString, "%04X", (int)(TempSensor_Reading / 0.125));
      Serial.print(tempString);
      Serial.print("#");
    }

    // firmware value
    if (!strcasecmp(cmd, "GV")) {
      Serial.print("11#");
    }

    // set the temperature coefficient
    if (!strcasecmp(cmd, "SC")) {
      TempCoefficientRaw = hexstr2long(param);
      // covert signed 8-bit to signed int
      if ((TempCoefficientRaw & 0x80)) {// negtive
        TempCoefficient = TempCoefficientRaw - 256;
      }
      else {
        TempCoefficient = TempCoefficientRaw;
      }
    }

    // set speed, only acceptable values are 02, 04, 08, 10, 20
    if (!strcasecmp(cmd, "SD"))
    {
      //char tempString[32];
      //sprintf(tempString, "%s = 0x%02X = %d = %d", param, SpeedFactorRaw, SpeedFactor, SpeedFactor * SPEEDMULT);
      //Serial.print(tempString);

      param[2] = '\0'; // Clamp parameter, else will end up with << 8
      SpeedFactorRaw = hexstr2long(param);

      // SpeedFactor: smaller value means faster
      SpeedFactor = 32 / SpeedFactorRaw;

      stepper.setMaxSpeed( SpeedFactor * SPEEDMULT );
    }

    // set full step mode
    if (!strcasecmp(cmd, "SF")) {
      // do nothing
    }

    // set half step mode
    if (!strcasecmp(cmd, "SH")) {
      // do nothing
    }

    // reset compatability mode
    if (!strcasecmp(cmd, "YM")) {
      MoonliteMode = false;
    }

    // set current motor position
    if (!strcasecmp(cmd, "SP")) {
      CurrentPosition = hexstr2long(param);
      stepper.setCurrentPosition(CurrentPosition);
    }

    // set new motor position
    if (!strcasecmp(cmd, "SN")) {
      // Ingore move command when Temp Comp is enabled
      if (!TempCompEn) {
        TargetPosition = hexstr2long(param);
        //stepper.moveTo(TargetPosition);
      }
    }

    // enable TempComp
    if (!strcasecmp (cmd, "Y+")) {
      TempCompEn = true;

      // Latch current position and average temperature.
      TempCompOriginalTemperature = TempSensor_Average;
      TempCompOriginalPosition = stepper.currentPosition();

      TempCompLastTemperature = TempSensor_Average;
      TempCompTargetPosition = TempCompOriginalPosition;
    }

    // disable TempComp, currently not used
    if (!strcasecmp (cmd, "Y-")) {
      TempCompEn = false;
    }

    if (!strcasecmp(cmd, "Z+")) {
      if (TempCompEn) {
        Serial.print("01#");
      }
      else {
        Serial.print("00#");
      }
    }

    // LED backlight value, always return "00"
    if (!strcasecmp(cmd, "GB")) {
      Serial.print("00#");
    }

    // home the motor, hard-coded, ignore parameters since we only have one motor
    if (!strcasecmp(cmd, "PH")) {
      stepper.setCurrentPosition(8000);
      stepper.moveTo(0);
      isRunning = true;
    }

    // set backlash
    if (!strcasecmp(cmd, "YB")) {
      Backlash = hexstr2long(param);
    }

    // get backlash set by YB
    if (!strcasecmp(cmd, "ZB")) {
      //char tempString[6];
      sprintf(tempString, "%02X", Backlash);
      Serial.print(tempString);
      Serial.print("#");
    }

    // set TempComp threshold in unit of 0.25 degree
    if (!strcasecmp(cmd, "YT")) {
      TempCompThresholdRaw = hexstr2long(param);
      TempCompThreshold = (float)TempCompThresholdRaw / 4; // covert to degree
    }

    // get TempComp threshold set by YT
    if (!strcasecmp(cmd, "ZT")) {
      //char tempString[6];
      sprintf(tempString, "%02X", TempCompThresholdRaw);
      Serial.print(tempString);
      Serial.print("#");
    }

    if (!strcasecmp(cmd, "YX")) {
      MaxSteps = hexstr2long(param);
    }

    if (!strcasecmp(cmd, "ZX")) {
      //char tempString[6];
      sprintf(tempString, "%04X", MaxSteps);
      Serial.print(tempString);
      Serial.print("#");
    }

    if (!strcasecmp(cmd, "ZA")) {
      int TempInt;
      TempInt = (int)(TempSensor_Average * 100);
      if (TempInt >= 0) {
        TempInt = TempInt & 0xFFFF;
      }
      else { // convert to 2's complement
        TempInt = ~abs(TempInt) & 0xFFFF;
      }

      //char tempString[6];
      sprintf(tempString, "%04X", TempInt);
      Serial.print(tempString);
      Serial.print("#");
    }

    // Debug Info
    if (!strcasecmp(cmd, "SS"))
      if (debug) outputDebugInfo();
  }

  unsigned long now = millis();

  isRunning = stepper.targetPosition() != stepper.currentPosition();

  // move motor if not done
  static bool stopStepperDone = false;
  if (isRunning)
  {
    stepper.run();
    stopStepperDone = false;

    static unsigned long lastprint = 0;
    if (debug && now - lastprint > 1000)
    {
      outputDebugState(' ');
      lastprint = now;
    }
  }
#if defined(OUTWARD_BACKLASH)
  // If motor is not moving but we need to apply backlash
  else if (Backlash != 0)
  {
    CurrentPosition = stepper.currentPosition();
    TargetPosition = OUTWARDS_BY(CurrentPosition, Backlash);
    stepper.enableOutputs();
    stepper.moveTo(TargetPosition);
    Backlash = 0;
  }
#endif
  // if motor is not moving
  else
  {
    // Turn off driver to save power if it is immobile for enough time
    if (!stopStepperDone && now - millisLastMove > STEPPER_DISABLEDELAY)
    {
      stepper.disableOutputs();
      if (debug) outputDebugState('.');

      // Save current location in EEPROM
      CurrentPosition = stepper.currentPosition();
      if (lastSavedPosition != CurrentPosition)
      {
        EEPROM.put(EEPROM_POS_LOC, CurrentPosition);
        lastSavedPosition = CurrentPosition;
        EEPROM.put(EEPROM_POS_BACKLASH, Backlash);
      }

      millisLastMove = now;
      stopStepperDone = true;
    }

    // TempComp average temperature calculation
    // Read one sample every 5s.
    if (now - millisLastTempSensorLatch > TEMPSENSOR_SAMPLEDELAY)
    {
      millisLastTempSensorLatch = now;

      // shift all the samples to the left - entry 0 has latest reading.
      for (i = TEMPSENSOR_ARRAY_SIZE - 1; i > 0; i--) {
        TempSensor_Array[i] = TempSensor_Array[i - 1];
        TempSensor_Valid_Array[i] = TempSensor_Valid_Array[i - 1];
      }
      TempSensor_Array[0] = TempSensor_Reading;
      TempSensor_Valid_Array[0] = true;

      // Calculate the average temperature
      // use Valid array to indicate whether an entry has valid data, to speed up calculation when power on.
      TempSensor_Array_Total = 0;
      TempSensor_Valid_Total = 0;
      for (i = 0; i < TEMPSENSOR_ARRAY_SIZE; i++) {
        if (TempSensor_Valid_Array[i]) {
          TempSensor_Array_Total += TempSensor_Array[i];
          TempSensor_Valid_Total ++;
        }
      }
      TempSensor_Average = TempSensor_Array_Total / TempSensor_Valid_Total;
    }

    // Read temperature periodically if driver/app does not initiate temperature read
    if (now - millisLastTempSensorRead > TEMPSENSOR_READDELAY) {
      millisLastTempSensorRead = now;

      if (TempSensor_Present)
      {
        TempSensor_Reading = DHT_getTemperature();
        HumiSensor_Reading = DHT_getHumidity();
      }
    }

  } // DistanceToGo == 0

  // TempComp focuser move
  // currently it only moves focuser in one direction, after temperature has dropped more than threshold, but report error (light pin13 LED on Nano board) if temperature has gone up over the hysteresis setting.
  // I have seen that there might be temperary temperature rise by 1 degree or so but it is very rare and usually it goes back down within 30min or so, that is the reason that is does not implement "back up" function.
  if (TempCompEn) {
    float TempCompTempChange = TempSensor_Average - TempCompLastTemperature;

    // debug use only
    //if (abs(TempCompTempChange) > TempCompThreshold) {
    // Calculate new position when temperature changes (drops) more than threshold
    if (TempCompTempChange < -TempCompThreshold) {
      //TargetPosition = TempCompLastPosition + (int)((TempSensor_Average - TempCompLastTemperature) * TempCoefficient);

      TempCompLastTemperature = TempSensor_Average;
      TempCompTargetPosition = TempCompOriginalPosition + (int)((TempSensor_Average - TempCompOriginalTemperature) * TempCoefficient);

      TempCompError = false;
    }
    // report error if temperature has gone up more than Hysteresis
    // there is a LEC on  pin13
    else if (TempCompTempChange > TEMPCOMP_HYSTERESIS) {
      //digitalWrite(PIN_OUTPUT_ERROR, HIGH);
      TempCompError = true;
    }
    else
    {
      //digitalWrite(PIN_OUTPUT_ERROR, LOW);
      TempCompError = false;
    }

    // Move focuser one step at a time with delay of TEMPCOMP_MOVEDELAY
    // It may be ok to move all steps at once with accelstepper, but it is better to have larger delay when taking images.
    if (millis() - millisLastMove > TEMPCOMP_MOVEDELAY) {
      if (stepper.currentPosition() < TempCompTargetPosition) {
        stepper.enableOutputs();
        stepper.move(1);
      }
      if (stepper.currentPosition() > TempCompTargetPosition) {
        stepper.enableOutputs();
        stepper.move(-1);
      }
    }
  }
  /*
    // disable manual movement when Temp Comp is enabled
    else if (BUT_MOVEMENT_ENABLED) { //TempCompEn
    // forward move
    if (digitalRead(PIN_INPUT_BUT_FW) == BUT_READING_RELEASED) {
      if (lastReadingButFW == BUT_READING_PRESSED) {
        stepper.stop();
      }
      lastReadingButFW = BUT_READING_RELEASED;
    }
    else {
      if (lastReadingButFW == BUT_READING_RELEASED) {
        stepper.enableOutputs();
        millisButFWPressed = millis();
      }
      // To not run over MaxSteps.
      long NewStep = min (pow(10, min(2, (int)((millis() - millisButFWPressed) / 1000))) * 10, MaxSteps - stepper.currentPosition());

      stepper.move(NewStep);
      millisLastMove = millis();
      lastReadingButFW = BUT_READING_PRESSED;
    }

    // backward moves
    if (digitalRead(PIN_INPUT_BUT_BW) == BUT_READING_RELEASED) {
      if (lastReadingButBW == BUT_READING_PRESSED) {
        stepper.stop();
      }
      lastReadingButBW = BUT_READING_RELEASED;
    }
    else {
      if (lastReadingButBW == BUT_READING_RELEASED) {
        stepper.enableOutputs();
        millisButBWPressed = millis();
      }
      // To not run under MinSteps (0).
      long NewStep = min (pow(10, min(2, (int)((millis() - millisButFWPressed) / 1000))) * 10, stepper.currentPosition());

      stepper.move(-NewStep);
      millisLastMove = millis();
      lastReadingButBW = BUT_READING_PRESSED;
    }
    }  // TempCompEn
  */
  blinkLED();
} // end loop

// Blink LED for status:
// blink 0.25s lit every 4s: Gathering temperature
// blink 1s lit every 4s: Average temperature acquired
// blink 2s lit every 4s: TempComp Enabled
// flashing: TempComp Error

void blinkLED ()
{
  int blinkMode; //0: blink every other slot, 1: blink while less than
  int blinkDutyCycle;

  if ((millis() - millisLastLEDBlink) > LEDBLINK_INTERVAL)
  {
    millisLastLEDBlink = millis();
    if (blinkTimer >= LEDBLINK_CYCLE - 1)
    {
      blinkTimer = 0;
    }
    else {
      blinkTimer ++;
    }
  }
  else
  {
    return;
  }

  if (TempCompEn)
  {
    if (TempCompError)
    {
      blinkMode = 0;
      blinkDutyCycle = 2;
    }
    else
    {
      blinkMode = 1;
      blinkDutyCycle = LEDBLINK_CYCLE / 2;
    }
  }
  else
  {
    if (TempSensor_Valid_Array[TEMPSENSOR_ARRAY_SIZE - 1])
    {
      blinkMode = 1;
      blinkDutyCycle = LEDBLINK_CYCLE / 4;
    }
    else if (TempSensor_Valid_Array[0]) {
      blinkMode = 1;
      blinkDutyCycle = 1;
    }
    else
    {
      blinkMode = 1;
      blinkDutyCycle = 0;
    }
  }

  if (blinkMode == 0) // blink every blinkDutyCycle
  {
    digitalWrite (PIN_OUTPUT_STATUS, blinkTimer % blinkDutyCycle != 0);
  }
  else // blink when less than blinkDutyCycle
  {
    digitalWrite (PIN_OUTPUT_STATUS, blinkTimer < blinkDutyCycle);
  }
}

// read the command until the terminating # character
void serialEvent () {
  while (Serial.available() && !eoc) {
    inChar = Serial.read();
    if (inChar != '#' && inChar != ':') {
      packet[idx++] = inChar;
      if (idx >= MAXCOMMAND) {
        idx = MAXCOMMAND - 1;
      }
    }
    else {
      if (inChar == '#') {
        eoc = true;
      }
    }
  }
}

long hexstr2long(char *line) {
  long ret = 0;

  ret = strtol(line, NULL, 16);
  return (ret);
}

float DHT_getTemperature()
{
  if (TempSensor_Present)
  {
    sensors_event_t e;
    TempSensor.temperature().getEvent(&e);
    return isnan(e.temperature) ? 0.0 : e.temperature;
  }
  return 0.0;
}

float DHT_getHumidity()
{
  if (TempSensor_Present)
  {
    sensors_event_t e;
    TempSensor.humidity().getEvent(&e);
    return isnan(e.relative_humidity) ? 0.0 : e.relative_humidity;
  }
  return 0.0;
}

void outputDebugState(char action)
{
  char tempString[128];
  snprintf(tempString, sizeof(tempString), "%08ld: %03d.%02d*C %03d.%02d%% %04X - %04X (B%+02d) = %04X @ %06d ; %04X - %04Xd = %04X @ %06d %c",
           millis(),
           (int)TempSensor_Reading, (int)(TempSensor_Reading * 100) % 100,
           (int)HumiSensor_Reading, (int)(HumiSensor_Reading * 100) % 100,
           (int)stepper.currentPosition(), (int)stepper.targetPosition(), (int)Backlash, (int)stepper.distanceToGo(), (int)stepper.speed(),
           action);
  Serial.println(tempString);
}

void outputDebugInfo()
{
  Serial.print("Temperature Sensor\n Present: "); Serial.print(TempSensor_Present); Serial.print("\n");
  Serial.print(" T: "); Serial.print(TempSensor_Reading); Serial.print("\n");
  Serial.print(" Coefficient: "); Serial.print(TempCoefficient); Serial.print("\n");
  for (i = 0; i < TEMPSENSOR_ARRAY_SIZE; i++)
  {
    Serial.print(" T#"); Serial.print(i); Serial.print(": ");
    Serial.print(TempSensor_Valid_Array[i]); Serial.print(" ");
    Serial.println(TempSensor_Array[i]);
  }
  Serial.print(" Average: ");
  Serial.println(TempSensor_Average);

  Serial.print("Compensation\n Last T: "); Serial.println(TempCompLastTemperature);
  Serial.print(" Original T: "); Serial.println(TempCompOriginalTemperature);
  Serial.print(" Original Position: "); Serial.println(TempCompOriginalPosition);
  Serial.print(" Target Position: "); Serial.println(TempCompTargetPosition);

  //Serial.print("Last Position");
  //Serial.print(TempCompLastPosition);
  //Serial.print("\n");

  Serial.print("Steppers\n Focuser Position: "); Serial.println(stepper.currentPosition());

  Serial.print("Potentiometer\n Current Reading: ");
  //Serial.print(analogRead(PIN_INPUT_POTENTION));
  Serial.print("\n");

  //Serial.print("speed");
  //Serial.print(MAXSPEED*(float)analogRead(PIN_INPUT_POTENTION)/1024);
  //Serial.print("\n");

  Serial.print("Buttons\n FW: ");
  if (!BUT_MOVEMENT_ENABLED) Serial.print("(disabled) ");
  Serial.println(digitalRead(PIN_INPUT_BUT_FW));

  Serial.print(" BW: ");
  if (!BUT_MOVEMENT_ENABLED) Serial.print("(disabled) ");
  Serial.println(digitalRead(PIN_INPUT_BUT_BW));
}

関連記事

コメント

この記事へのコメントはありません。

TOP