Go to Qiita Advent Calendar 2024 Top
LoginSignup
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?

More than 5 years have passed since last update.

@masashi_214

DCC サーボモータファンクションデコーダ(DCC Servo Function Decoder)

Posted at

###概要
Smile Function Decoderを使ってファンクションデコーダタイプのサーボモータデコーダを考えました。

###動作
・初期DCCアドレスは3、ファンクションアドレスは４
・ベタ書きだと、フラッシュに入りきらない為、収めるために見辛いプログラムになっています。
digitalWrite→PORTB、複数回呼ばれる関数をまとめたり、機能削ったり。

###今後
・周知なバグ
　なぜかファンクションアドレス２番が受信できない？！

###リンク
https://twitter.com/masashi_214
http://ayabu.blog.shinobi.jp/
http://dcc.client.jp/
http://www007.upp.so-net.ne.jp/nagoden/

ソースファイルはグーグルドライブで公開
https://drive.google.com/open?id=1uVG4coTg2dy28j-0UsSH-mB7xH4uWr5C

// DCC Servo Function Decoder Rev 1 for DS-DCC decode
// http://dcc.client.jp
// http://ayabu.blog.shinobi.jp

#include "NmraDcc.h"
#include <arduino.h>
//#include <SoftwareSerial.h>

//各種設定、宣言

#define DECODER_ADDRESS 3
#define DCC_ACK_PIN 0   // Atiny85 PB0(5pin) if defined enables the ACK pin functionality. Comment out to disable.
//                      // Atiny85 DCCin(7pin)
#define O1 0            // Atiny85 PB0(5pin)             head sign light
#define O2 1            // Atiny85 PB1(6pin) analogwrite Head light
#define O3 3            // Atint85 PB3(2pin)             tail light
#define O4 4            // Atiny85 PB4(3pin) analogwrite room light

#define ON 1
#define OFF 0
#define UP 1
#define DOWN 0

/*
#define MAX_PWMDUTY 255
#define MID_PWMDUTY 10
*/
#define CV_VSTART		2
#define CV_ACCRATIO		3
#define CV_DECCRATIO	4
#define CV_F0_FORWARD 33
#define CV_F0_BACK 34
#define CV_F1 35
#define CV_F2 36
#define CV_F3 37
#define CV_F4 38
#define CV_F5 39
#define CV_F6 40
#define CV_F7 41
#define CV_F8 42
#define CV_F9 43
#define CV_F10 44
#define CV_F11 45
#define CV_F12 46
#define CV_49_F0_FORWARD_LIGHT 49
#define CV_DIMMING_SPEED 50
#define CV_DIMMING_LIGHT_QUANTITY 51
#define CV_ROOM_DIMMING 52

#define CV_zeroDeg 60       // 0deg時のPWMの値
#define CV_ninetyDeg 61     // 90deg時のPWMの値
#define CV_onDeg 62         // on時の角度
#define CV_offDeg 63        // off時の角度
#define CV_initDeg 64       // 起動時の角度
#define CV_onSpeed 65       // off->onに移行する時間
#define CV_offSpeed 66      // on->offに移行する時間
#define CV_sdir 67          // 最後のdir保持用
#define CV_function 68   // サーボモータファンクッション番号
#define CV_dummy 69         // dummy
#define MAN_ID_NUMBER 166   // Manufacture ID //
#define MAN_VER_NUMBER  01  // Release Ver CV07 //


//使用クラスの宣言
NmraDcc	 Dcc;
DCC_MSG	 Packet;
//SoftwareSerial mySerial(O1, O2); // RX, TX

//Task Schedule
unsigned long gPreviousL5 = 0;


//Function State
uint8_t gState_F0 = 0;
uint8_t gState_F1 = 0;
uint8_t gState_F2 = 0;
uint8_t gState_F3 = 0;
uint8_t gState_F4 = 0;
uint8_t gState_F5 = 0;
uint8_t gState_F6 = 0;
uint8_t gState_F7 = 0;
uint8_t gState_F8 = 0;
uint8_t gState_F9 = 0;
uint8_t gState_F10 = 0;
uint8_t gState_F11 = 0; 
uint8_t gState_F12 = 0;
uint8_t gState_F13 = 0;
uint8_t gState_F14 = 0; 
uint8_t gState_F15 = 0;
uint8_t gState_F16 = 0;
uint8_t gState_Function = 0;

//CV related
uint8_t gCV1_SAddr = 3; 
uint8_t gCVx_LAddr = 3;
uint8_t gCV29_Conf = 0;
uint8_t gCV68_servoAdder = 4; // サーボアドレス
uint8_t gCV49_fx = 20;

uint8_t zeroDeg = 0;        // 0degのPWM値
uint8_t ninetyDeg = 0;      // 90degのPWM値 ※ HK-5320は0-60deg DM-S0025は0-180deg
uint8_t onDeg = 0;          // ON時の角度
uint8_t offDeg = 0;         // OFF時の角度
uint8_t initDeg = 0;        // 電源切る前の角度
uint8_t onSpeed = 0;        // OFF->ONのスピード
uint8_t offSpeed = 0;       // ON->OFFのスピード
uint8_t sdir = 0;           // gDirの最新値保存用
float nowDeg = 0;
float delt_deg = 0;          // 10ms辺りの変化量
    
//Internal variables and other.
#if defined(DCC_ACK_PIN)
const int DccAckPin = DCC_ACK_PIN ;
#endif

struct CVPair {
  uint16_t	CV;
  uint8_t	Value;
};
CVPair FactoryDefaultCVs [] = {
  {CV_MULTIFUNCTION_PRIMARY_ADDRESS, DECODER_ADDRESS}, // CV01
  {CV_ACCESSORY_DECODER_ADDRESS_MSB, 0},               // CV09 The LSB is set CV 1 in the libraries .h file, which is the regular address location, so by setting the MSB to 0 we tell the library to use the same address as the primary address. 0 DECODER_ADDRESS
  {CV_MULTIFUNCTION_EXTENDED_ADDRESS_MSB, 0},          // CV17 XX in the XXYY address
  {CV_MULTIFUNCTION_EXTENDED_ADDRESS_LSB, 0},          // CV18 YY in the XXYY address
//  {CV_29_CONFIG, 128 },                                // CV29 Make sure this is 0 or else it will be random based on what is in the eeprom which could caue headaches
  {CV_zeroDeg ,8},                        // CV60 8で約500us
  {CV_ninetyDeg ,36},                     // CV61 36で約2400us DM-S0025は36以上に設定すると動きがおかしくなる
  {CV_onDeg ,180},                        // CV62 on時の角度
  {CV_offDeg  ,0},                        // CV63 off時の角度
  {CV_initDeg ,0},                        // CV64 電源切る前の角度
  {CV_onSpeed, 20},                       // CV65 20で2000msec
  {CV_offSpeed, 20},                      // CV66 20で2000msec
  {CV_sdir , 0},                          // CV67 0
  {CV_function , 4},                    // CV68 servo Address 4
  {CV_dummy,0},
};

void(* resetFunc) (void) = 0;  //declare reset function at address 0

uint8_t FactoryDefaultCVIndex = sizeof(FactoryDefaultCVs) / sizeof(CVPair);

void notifyDccReset(uint8_t hardReset );

//uint16_t limitSpeed(uint16_t inSpeed);

void notifyCVResetFactoryDefault()
{
  //When anything is writen to CV8 reset to defaults.

  resetCVToDefault();
  delay(1000);  //typical CV programming sends the same command multiple times - specially since we dont ACK. so ignore them by delaying
  resetFunc();
};

//------------------------------------------------------------------
// Resrt
//------------------------------------------------------------------
void notifyDccReset(uint8_t hardReset )
{
#if 0  
  digitalWrite(O1,LOW);
  digitalWrite(O2,LOW);
  digitalWrite(O3,LOW);
  digitalWrite(O4,LOW);

  gState_F0 = 0; // ADD
  gState_F1 = 0;
  gState_F2 = 0;
  gState_F3 = 0; 
  gState_F4 = 0;
  gState_F5 = 0;
#endif
}

//------------------------------------------------------------------
// CVをデフォルトにリセット(Initialize cv value)
// Serial.println("CVs being reset to factory defaults");
//------------------------------------------------------------------
void resetCVToDefault()
{
  for (int j = 0; j < FactoryDefaultCVIndex; j++ ) {
    Dcc.setCV( FactoryDefaultCVs[j].CV, FactoryDefaultCVs[j].Value);
  }
};



extern void	   notifyCVChange( uint16_t CV, uint8_t Value) {
  //CVが変更されたときのメッセージ
  //Serial.print("CV ");
  //Serial.print(CV);
  //Serial.print(" Changed to ");
  //Serial.println(Value, DEC);
};

//------------------------------------------------------------------
// CV Ack
//------------------------------------------------------------------
void notifyCVAck(void)
{
//サーボモータを動かすとギミックを壊しかねないのでコメントアウト
//Serial.println("notifyCVAck");
#if 0 
  digitalWrite(O1,HIGH);
  digitalWrite(O2,HIGH);
  digitalWrite(O3,HIGH);
  digitalWrite(O4,HIGH);
  delay( 6 );
  digitalWrite(O4,LOW);
  digitalWrite(O3,LOW);
  digitalWrite(O2,LOW);
  digitalWrite(O1,LOW);
#endif
//MOTOR_Ack();
}

void MOTOR_Ack(void)
{
  analogWrite(O4, 128);
  delay( 6 );  
  analogWrite(O4, 0);
}

//------------------------------------------------------------------
// Arduino固有の関数 setup() :初期設定
//------------------------------------------------------------------
void setup()
{
//  pinMode(O1, OUTPUT);
//  pinMode(O2, OUTPUT);
//  pinMode(O3, OUTPUT);
//  pinMode(O4, OUTPUT);
DDRB = (1<<DDB3)|(1<<DDB2)|(1<<DDB1)|(1<<DDB0); 

  //DCCの応答用負荷ピン
#if defined(DCCACKPIN)
  //Setup ACK Pin
  pinMode(DccAckPin, OUTPUT);
  digitalWrite(DccAckPin, 0);
#endif

//mySerial.begin(9600);
//mySerial.println("Hello,SFD");

#if !defined(DECODER_DONT_DEFAULT_CV_ON_POWERUP)
  if ( Dcc.getCV(CV_MULTIFUNCTION_PRIMARY_ADDRESS) == 0xFF ) {	 //if eeprom has 0xFF then assume it needs to be programmed
    //Serial.println("CV Defaulting due to blank eeprom");
    notifyCVResetFactoryDefault();

  } else {
    //Serial.println("CV Not Defaulting");
  }
#else
  //Serial.println("CV Defaulting Always On Powerup");
  notifyCVResetFactoryDefault();
#endif

  // Setup which External Interrupt, the Pin it's associated with that we're using, disable pullup.
  Dcc.pin(0, 2, 0); // Atiny85 7pin(PB2)をDCC_PULSE端子に設定

  // Call the main DCC Init function to enable the DCC Receiver
  Dcc.init( MAN_ID_NUMBER, 100,   FLAGS_MY_ADDRESS_ONLY , 0 );

  //Init CVs
  gCV1_SAddr = Dcc.getCV( CV_MULTIFUNCTION_PRIMARY_ADDRESS ) ;

  //Init CVs
  gCV1_SAddr = Dcc.getCV( CV_MULTIFUNCTION_PRIMARY_ADDRESS ) ;
  gCVx_LAddr = (Dcc.getCV( CV_MULTIFUNCTION_EXTENDED_ADDRESS_MSB ) << 8) + Dcc.getCV( CV_MULTIFUNCTION_EXTENDED_ADDRESS_LSB );
//gCV29_Conf = Dcc.getCV( CV_29_CONFIG );
  gCV68_servoAdder = Dcc.getCV( CV_function );

  //Init CVs
  zeroDeg = Dcc.getCV( CV_zeroDeg );
  ninetyDeg = Dcc.getCV( CV_ninetyDeg );
  onDeg = Dcc.getCV( CV_onDeg );
  offDeg = Dcc.getCV( CV_offDeg );
  initDeg = Dcc.getCV( CV_initDeg );
  onSpeed = Dcc.getCV( CV_onSpeed );  
  offSpeed = Dcc.getCV( CV_offSpeed );
  sdir = Dcc.getCV( CV_sdir );

  GTCCR = 1 << PWM1B | 2 << COM1B0;
  TCCR1 = 11 << CS10;

  //Reset task
  gPreviousL5 = millis();
}

//---------------------------------------------------------------------
// Arduino main loop
//---------------------------------------------------------------------
void loop() {
  // You MUST call the NmraDcc.process() method frequently from the Arduino loop() function for correct library operation
  Dcc.process();

  if ( (millis() - gPreviousL5) >= 10) // 100:100msec  10:10msec  Function decoder は 10msecにしてみる。
  {
    ServoControl();
    //Reset task
    gPreviousL5 = millis();
  }
}


//---------------------------------------------------------------------
// Servo control Task (100Hz:100ms)
//---------------------------------------------------------------------
void writeCV()
{
  Dcc.setCV(CV_sdir,gState_Function);        // 最終値のアクセサリ番号をCV_sdirに書き込み
}
void anaWR()
{
  analogWrite(O4, map(nowDeg,0,180, zeroDeg, ninetyDeg));
}

void gState()
{
  switch( gCV68_servoAdder ){
    case 0:
            gState_Function = gState_F0;
            break;
    case 1:
            gState_Function = gState_F1;
            break;
    case 2:
            gState_Function = gState_F2;
            break;
    case 3:
            gState_Function = gState_F3;
            break;
    case 4:
            gState_Function = gState_F4;
            break;
    case 5:
            gState_Function = gState_F5;
            break;
    case 6:
            gState_Function = gState_F6;
            break;
    case 7:
            gState_Function = gState_F7;
            break;
    case 8:
            gState_Function = gState_F8;
            break;
    case 9:
            gState_Function = gState_F9;
            break;
    case 10:
            gState_Function = gState_F10;
            break;
    case 11:
            gState_Function = gState_F11;
            break;
    case 12:
            gState_Function = gState_F12;
            break;

    default:
            break;                    
  }
}

void ServoControl()
{
  enum{
      ST_STANDABY = 0,
      ST_IDLE,
      ST_ON,
      ST_ON_RUN,
      ST_OFF,
      ST_OFF_RUN,
  };
  
  static char state = ST_STANDABY;        // ステート

  static char updown_flg = 0;         // 0:up 1:down

  gState();

  switch(state){
      case ST_STANDABY:               // 起動時一回だけの処理
        if(gState_Function == sdir ){ // 前回最後のSTR/DIVが同じ？
          if(gState_Function == 0){   // OFF?
            nowDeg = offDeg;     
            PORTB |= _BV(PB0);  // digitalWrite(O1, HIGH);
            PORTB &= ~_BV(PB1); // digitalWrite(O2, LOW);
          } else {                    // ON?
            nowDeg = onDeg;  
            PORTB &= ~_BV(PB0); // digitalWrite(O1, LOW);
            PORTB |= _BV(PB1);  // digitalWrite(O2, HIGH);
          }
          state = ST_IDLE;
          break;
        } else {
          if(sdir != 0 and gState_Function == 0){
              state = ST_OFF;             
          } else {
            state = ST_ON;
          }
        }
        break;
      case ST_IDLE: // 1 
            if(gState_Function == 0 ){           // Servo O4:OFF
              if(nowDeg == offDeg){   // 最終値まで行っていたら抜ける
                state = ST_IDLE;
                return;
              }
              state = ST_OFF;
            } else if(gState_Function != 1 ){    // Servo O4:ON
              if(nowDeg == onDeg){    // 最終値まで行っていたら抜ける
                state = ST_IDLE;
                return;
              }
              state = ST_ON;
            }
            break;

      case ST_ON: //11: // ON処理
            if(onDeg - offDeg > 0){  // 上昇か、下降か確認
              updown_flg = UP;       // 上昇
              delt_deg = (float)abs(onDeg - offDeg) / onSpeed / 10; // offDegからonDegまでの移動量を算出
            } else {
for(;;);
//              updown_flg = DOWN;       // 下降
//              delt_deg = (float)abs(onDeg - offDeg) / offSpeed / 10; // offDegからonDegまでの移動量を算出
            }
            
            nowDeg = offDeg;        // 現在の角度を導入
            anaWR();
            state = ST_ON_RUN;
            break;
            
      case ST_ON_RUN: // 12
              if(updown_flg == UP) {
                nowDeg = nowDeg + delt_deg;       // 上昇
                if(nowDeg > onDeg){
                  nowDeg = onDeg;
                  anaWR();
                  writeCV();
                  PORTB &= ~_BV(PB0); // digitalWrite(O1, LOW);
                  PORTB |= _BV(PB1);  // digitalWrite(O2, HIGH);
                  state = ST_IDLE;                  
                } else {
                 anaWR();          
                  state = ST_ON_RUN;                  
                }
              } else {
                nowDeg = nowDeg - delt_deg;       // 下降   
               if(nowDeg < onDeg){
                  nowDeg = onDeg;
                  anaWR();
                  writeCV();
                  PORTB |= _BV(PB0);  // digitalWrite(O1, HIGH);
                  PORTB &= ~_BV(PB1); // digitalWrite(O2, LOW);
                  state = ST_IDLE;   
                } else {
                  anaWR();              
                 state = ST_ON_RUN;                    
               }
              }
            break;

      case ST_OFF: //21:  // ON->OFF処理

            if(onDeg - offDeg > 0){  // 上昇か、下降か確認
              updown_flg = DOWN;       // 下昇
              delt_deg = (float)abs(onDeg - offDeg) / offSpeed / 10; // offDegからonDegまでの移動量を算出
            } else {
for(;;);
//              updown_flg = UP;       // 上降
//              delt_deg = (float)abs(onDeg - offDeg) / onSpeed / 10; // offDegからonDegまでの移動量を算出
            }
            nowDeg = onDeg;        // 現在の角度を導入
            anaWR();
            state = ST_OFF_RUN;
            break;
            
      case ST_OFF_RUN: //22
              if(updown_flg == UP) {
                nowDeg = nowDeg + delt_deg;       // 上昇
                if(nowDeg > offDeg){
                  nowDeg = offDeg;
                  anaWR();
                  writeCV();
                  PORTB &= ~_BV(PB0); // digitalWrite(O1, LOW);
                  PORTB |= _BV(PB1);  // digitalWrite(O2, HIGH);
                  state = ST_IDLE;                  
                } else {
                  anaWR();
                  state = ST_OFF_RUN;                  
                }
              } else {
                nowDeg = nowDeg - delt_deg;       // 下降
                if(nowDeg < offDeg){
                  nowDeg = offDeg;
                  anaWR();  
                  writeCV();
                  PORTB |= _BV(PB0);  // digitalWrite(O1, HIGH);
                  PORTB &= ~_BV(PB1); // digitalWrite(O2, LOW);
                  state = ST_IDLE;   
                } else {
                  anaWR();             
                  state = ST_OFF_RUN;                    
                }
              }
            break;
            
      default:
            break;
  }   
}

//DCC速度信号の受信によるイベント
//extern void notifyDccSpeed( uint16_t Addr, uint8_t Speed, uint8_t ForwardDir, uint8_t MaxSpeed )
extern void notifyDccSpeed( uint16_t Addr, DCC_ADDR_TYPE AddrType, uint8_t Speed, DCC_DIRECTION Dir, DCC_SPEED_STEPS SpeedSteps )
{
}


//---------------------------------------------------------------------------
//ファンクション信号受信のイベント
//FN_0_4とFN_5_8は常時イベント発生（DCS50KはF8まで）
//FN_9_12以降はFUNCTIONボタンが押されたときにイベント発生
//前値と比較して変化あったら処理するような作り。
//---------------------------------------------------------------------------
//extern void notifyDccFunc( uint16_t Addr, FN_GROUP FuncGrp, uint8_t FuncState)
extern void notifyDccFunc(uint16_t Addr, DCC_ADDR_TYPE AddrType, FN_GROUP FuncGrp, uint8_t FuncState)
{
  
  if( FuncGrp == FN_0_4)
  {
    if( gState_F0 != (FuncState & FN_BIT_00))
    {
      //Get Function 0 (FL) state
      gState_F0 = (FuncState & FN_BIT_00);
    }
    if( gState_F1 != (FuncState & FN_BIT_01))
    {
      //Get Function 1 state
      gState_F1 = (FuncState & FN_BIT_01);
    }
    if( gState_F2 != (FuncState & FN_BIT_02))
    {
      gState_F2 = (FuncState & FN_BIT_02);
    }
    if( gState_F3 != (FuncState & FN_BIT_03))
    {
      gState_F3 = (FuncState & FN_BIT_03);
    }
    if( gState_F4 != (FuncState & FN_BIT_04))
    {
      gState_F4 = (FuncState & FN_BIT_04);
    }
  }

  if( FuncGrp == FN_5_8)
  {
    if( gState_F5 != (FuncState & FN_BIT_05))
    {
      //Get Function 0 (FL) state
      gState_F5 = (FuncState & FN_BIT_05);
    }
    if( gState_F6 != (FuncState & FN_BIT_06))
    {
      //Get Function 1 state
      gState_F6 = (FuncState & FN_BIT_06);
    }
    if( gState_F7 != (FuncState & FN_BIT_07))
    {
      gState_F7 = (FuncState & FN_BIT_07);
    }
    if( gState_F8 != (FuncState & FN_BIT_08))
    {
      gState_F8 = (FuncState & FN_BIT_08);
    }
  }
  if( FuncGrp == FN_9_12)
  {
    if( gState_F9 != (FuncState & FN_BIT_09))
    {
      gState_F9 = (FuncState & FN_BIT_09);
    }
    if( gState_F10 != (FuncState & FN_BIT_10))
    {
      gState_F10 = (FuncState & FN_BIT_10);
    }
    if( gState_F11 != (FuncState & FN_BIT_11))
    {
      gState_F11 = (FuncState & FN_BIT_11);
    }
    if( gState_F12 != (FuncState & FN_BIT_12))
    {
      gState_F12 = (FuncState & FN_BIT_12);
    }
  }
/*
  if( FuncGrp == FN_13_20)
  {
    if( gState_F13 != (FuncState & FN_BIT_13))
    {
      gState_F13 = (FuncState & FN_BIT_13);
    }
    if( gState_F14 != (FuncState & FN_BIT_14))
    {
      gState_F14 = (FuncState & FN_BIT_14);
    }
    if( gState_F15 != (FuncState & FN_BIT_15))
    {
      gState_F15 = (FuncState & FN_BIT_15);
    }
    if( gState_F16 != (FuncState & FN_BIT_16))
    {
      gState_F16 = (FuncState & FN_BIT_16);
    }
  }
*/   
}
0
0
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
0
0

Delete article

Deleted articles cannot be recovered.

Draft of this article would be also deleted.

Are you sure you want to delete this article?