/**************************************************************************** * (C) Tokyo Cosmos Electric, Inc. (TOCOS) - 2013 all rights reserved. * * Condition to use: * - The full or part of source code is limited to use for TWE (TOCOS * Wireless Engine) as compiled and flash programmed. * - The full or part of source code is prohibited to distribute without * permission from TOCOS. * ****************************************************************************/ //****************************************************************************************** // KOG RC RXAP10 （2.4GHz無線モジュールTWELITEを使ったRC受信機 , 超小型インドア飛行機用) // 2022/01/07 // 仕様 //　マグネットアクチュエータ信号出力　2ch //　動力用モータ：1ch //　デジタル出力:8ch //送信機から送られてくる4ch目のデータは使わない。 // //2022/09/01 サーボ信号にも対応できるように変更 // //****************************************************************************************** #include // C 標準ライブラリ用 #include // NXP ペリフェラル API 用 #include "Master.h" #include "utils.h" // ペリフェラル API のラッパなど #include "serial.h" // シリアル用 #include "sprintf.h" // SPRINTF 用 #include "common.h" #include "eeprom_6x.h" //プロトタイプ宣言 void _set_mode(void); void digi_out(uint8 digi_data); /****************************************************************************/ /*** ToCoNet Definitions ***/ /****************************************************************************/ // Select Modules (define befor include "ToCoNet.h") #define ToCoNet_USE_MOD_ENERGYSCAN #define ToCoNet_USE_MOD_NBSCAN // Neighbour scan module #undef ToCoNet_USE_MOD_NBSCAN_SLAVE #include "ToCoNet.h" #include "ToCoNet_mod_prototype.h" // ToCoNet モジュール定義 //#define UART_BAUD 115200 // シリアルのボーレート // ToCoNet 用パラメータ #define RX_ID 0xABCDEF12 //APP_ID 任意の32bitデータ。送信機と合わせること。 #define RX_CH 11 //11～26まで設定可。　送信機と合わせること。 #define u8TxPower 3 //3 : 送信パワー最大 //#define servo 0 //マグネットアクチュエータ出力の場合servo=0　、 サーボ出力の場合servo=1 //#define mix 1 //ミキシングなしの場合 mix=0　、 ミキシングありの場合 mix=1 //#define center 0 //動力用モータのOFFポイントが中央より下方の場合 center=0　、 動力用モータのOFFポイントが中央の場合の場合 center=1 static tsFILE sSerStream; // シリアル用ストリーム static tsSerialPortSetup sSerPort; // シリアルポートデスクリプタ static uint32 u32Seq; // 送信パケットのシーケンス番号 uint8 ad1; //ADC1のデータ int8 rcv_buf[64]; //受信データ uint8 rcv_tmr = 50; //受信確認用タイマ uint8 mctr=50; //モニタ出力間引き用 uint8 mon_tmr=100; //モーターONタイマ uint8 moff_tmr=100; //モーターOFFタイマ uint8 mon_flag=0x01; //モーターONフラグ uint8 mlost_flag=0x01; //信号ロストフラグ　モータ用 uint8 mbat_flag=0x01; //バッテリー残量フラグ uint8 mpwm_flag=0x01; //モーターPWMフラグ int16 mpwm_period=400; //モーターPWM周期 int16 mon_duty=0; //モーターON_duty uint8 mode_ctr[2] = {16,0}; //モードカウンタ（リバース、ミキシング用） uint8 set_flag = 0; //セッティング　フラグ uint8 set_tmr = 250; //リバース、ミキシング　モードセッティング用タイマ int16 ch1i = 1500; //パルス幅初期値(usec) int16 ch2i = 1500; //パルス幅初期値(usec) int16 ch3i = 1500; //パルス幅初期値(usec) int16 ch4i = 1500; //パルス幅初期値(usec) /****************************************************************************/ /*** Local Variables ***/ /****************************************************************************/ static tsAppData sAppData; //!< アプリケーションデータ @ingroup MASTER tsTimerContext sTimerApp; //!< タイマー管理構造体 @ingroup MASTER tsTimerContext sTimerPWM[4]; //!< タイマー管理構造体 @ingroup MASTER // デバッグ出力用に UART を初期化 static void vSerialInit() { static uint8 au8SerialTxBuffer[96]; static uint8 au8SerialRxBuffer[32]; sSerPort.pu8SerialRxQueueBuffer = au8SerialRxBuffer; sSerPort.pu8SerialTxQueueBuffer = au8SerialTxBuffer; sSerPort.u32BaudRate = UART_BAUD; sSerPort.u16AHI_UART_RTS_LOW = 0xffff; sSerPort.u16AHI_UART_RTS_HIGH = 0xffff; sSerPort.u16SerialRxQueueSize = sizeof(au8SerialRxBuffer); sSerPort.u16SerialTxQueueSize = sizeof(au8SerialTxBuffer); sSerPort.u8SerialPort = E_AHI_UART_0; sSerPort.u8RX_FIFO_LEVEL = E_AHI_UART_FIFO_LEVEL_1; SERIAL_vInit(&sSerPort); sSerStream.bPutChar = SERIAL_bTxChar; sSerStream.u8Device = E_AHI_UART_0; } //受け取ったADデータをサーボパルス幅信号に変換（マグネットアクチュエータの場合は不要） uint16 get_pulse(uint16 adi) { uint16 ui16_1 , ui16_2; ui16_1=(1400*adi/1024)+800; //サーボ回転方向：ノーマルの場合 //ui16_1=(-1400*adi/1024)+2200; //サーボ回転方向：リバースの場合 ui16_2=ui16_1 & 0x00000FFF; return ui16_2; } // ユーザ定義のイベントハンドラ static void vProcessEvCore(tsEvent *pEv, teEvent eEvent, uint32 u32evarg) { int8 i8_0[25] , i8_4[6]; int8 ui8_1; int16 si16_2 , si16_3 , si16_4 , si16_5 , hdri , douti , csumi; // 起動時 (delay_time 2.5msec from cold start) if (eEvent == E_EVENT_START_UP) { vfPrintf(&sSerStream , "KOG RC RXA0 RX_CH=%d , APP_ID=%X\r\n" , RX_CH , RX_ID); } // 20ms 周期のシステムタイマ通知 if (eEvent == E_EVENT_TICK_TIMER) { strncpy(i8_0,rcv_buf,24); //Headerの数値化処理 i8_4[0]='0'; //0Xを付加 (0xが頭にあると１６進数と判断される） i8_4[1]='x'; i8_4[2]=i8_0[0]; i8_4[3]=i8_0[1]; i8_4[4]='\0'; hdri=strtoul(i8_4,NULL,0); //１６進文字列を整数に変換。 //ch1 pulseの数値化処理 i8_4[2]=i8_0[3]; i8_4[3]=i8_0[4]; i8_4[4]=i8_0[5]; i8_4[5]='\0'; ch1i=strtoul(i8_4,NULL,0); //１６進文字列を整数に変換。 //ch2 pulseの数値化処理 i8_4[2]=i8_0[7]; i8_4[3]=i8_0[8]; i8_4[4]=i8_0[9]; i8_4[5]='\0'; ch2i=strtoul(i8_4,NULL,0); //１６進文字列を整数に変換。 //ch3 pulseの数値化処理 i8_4[2]=i8_0[11]; i8_4[3]=i8_0[12]; i8_4[4]=i8_0[13]; i8_4[5]='\0'; ch3i=strtoul(i8_4,NULL,0); //１６進文字列を整数に変換。 //ch4 pulseの数値化処理 i8_4[2]=i8_0[15]; i8_4[3]=i8_0[16]; i8_4[4]=i8_0[17]; i8_4[5]='\0'; ch4i=strtoul(i8_4,NULL,0); //１６進文字列を整数に変換。 //digital outputの数値化処理 i8_4[2]=i8_0[19]; i8_4[3]=i8_0[20]; i8_4[4]='\0'; douti=strtoul(i8_4,NULL,0); //１６進文字列を整数に変換。 //check sumの数値化処理 i8_4[2]=i8_0[22]; i8_4[3]=i8_0[23]; i8_4[4]='\0'; csumi=strtoul(i8_4,NULL,0); //１６進文字列を整数に変換。 _set_mode(); //セッティングモード if((hdri==0xAA) && (((hdri+ch1i+ch2i+ch3i+ch4i+douti+csumi) & 0x00FF) == 0x00)) //Header & チェックサム { if((u32AHI_DioReadInput() & 0x00040000) == 0x00040000) //DIO18 High:マグネットアクチュエータ出力 Low:サーボ出力 { //マグネットアクチュエータ出力の場合 if((u32AHI_DioReadInput() & 0x00000020) == 0x00000020) //DIO5 High:ノーマル出力 Low:ミキシング出力 { //マグネットアクチュエーター（ノーマル）の場合 switch(mode_ctr[0] & 0x03) { case 0: si16_4 = ch1i; //パルス幅に変換（ノーマル） si16_5 = ch2i; //パルス幅に変換（ノーマル） break; case 1: si16_4 = 511-(ch1i-511); //パルス幅に変換（リバース） si16_5 = ch2i; //パルス幅に変換（ノーマル） break; case 2: si16_4 = ch1i; //パルス幅に変換（ノーマル） si16_5 = 511-(ch2i-511); //パルス幅に変換（リバース） break; case 3: si16_4 = 511-(ch1i-511); //パルス幅に変換（リバース） si16_5 = 511-(ch2i-511); //パルス幅に変換（リバース） break; } } else { //マグネットアクチュエーター（ミキシング）の場合 si16_2 = ch1i - 511; //CH1 パルス幅に変換 si16_3 = ch2i - 511; //CH2 パルス幅に変換 switch(mode_ctr[0] & 0x0f) { case 0: si16_4 = (1023+(si16_2 + si16_3))/2; //ミキシング方向 si16_5 = (1023+(si16_2 + si16_3))/2; //ミキシング方向 break; case 1: si16_4 = (1023+(si16_2 + si16_3))/2; //ミキシング方向 si16_5 = (1023+(-si16_2 + si16_3))/2; //ミキシング方向 break; case 2: si16_4 = (1023+(si16_2 + si16_3))/2; //ミキシング方向 si16_5 = (1023+(si16_3 - si16_2))/2; //ミキシング方向 break; case 3: si16_4 = (1023+(si16_2 + si16_3))/2; //ミキシング方向 si16_5 = (1023+(-si16_2 - si16_3))/2; //ミキシング方向 break; case 4: si16_4 = (1023+(-si16_2 + si16_3))/2; //ミキシング方向 si16_5 = (1023+(si16_2 + si16_3))/2; //ミキシング方向 break; case 5: si16_4 = (1023+(-si16_2 + si16_3))/2; //ミキシング方向 si16_5 = (1023+(-si16_3 + si16_2))/2; //ミキシング方向 break; case 6: si16_4 = (1023+(-si16_3 + si16_2))/2; //ミキシング方向 si16_5 = (1023+(si16_2 - si16_3))/2; //ミキシング方向 break; case 7: si16_4 = (1023+(-si16_3 + si16_2))/2; //ミキシング方向 si16_5 = (1023+(-si16_2 - si16_3))/2; //ミキシング方向 break; case 8: si16_4 = (1023+(si16_3 - si16_3))/2; //ミキシング方向 si16_5 = (1023+(si16_3 + si16_2))/2; //ミキシング方向 break; case 9: si16_4 = (1023+(si16_2 - si16_3))/2; //ミキシング方向 si16_5 = (1023+(-si16_2 + si16_3))/2; //ミキシング方向 break; case 10: si16_4 = (1023+(si16_2 - si16_3))/2; //ミキシング方向 si16_5 = (1023+(si16_2 - si16_3))/2; //ミキシング方向 break; case 11: si16_4 = (1023+(si16_2 - si16_3))/2; //ミキシング方向 si16_5 = (1023+(-si16_3 - si16_2))/2; //ミキシング方向 break; case 12: si16_4 = (1023+(-si16_2 - si16_3))/2; //ミキシング方向 si16_5 = (1023+(si16_2 + si16_3))/2; //ミキシング方向 break; case 13: si16_4 = (1023+(-si16_2 - si16_3))/2; //ミキシング方向 si16_5 = (1023+(-si16_2 + si16_3))/2; //ミキシング方向 break; case 14: si16_4 = (1023+(-si16_2 - si16_3))/2; //ミキシング方向 si16_5 = (1023+(si16_3 - si16_2))/2; //ミキシング方向 break; case 15: si16_4 = (1023+(-si16_3 - si16_2))/2; //ミキシング方向 si16_5 = (1023+(-si16_2 - si16_3))/2; //ミキシング方向 break; } } //ch1 （マグネットアクチュエーター駆動） if(si16_4 < 511) { vAHI_TimerStartRepeat(E_AHI_TIMER_2,0,350); //off PWM2 vAHI_TimerStartRepeat(E_AHI_TIMER_1,511-si16_4,350); //on PWM1 } else if(511 < si16_4) { vAHI_TimerStartRepeat(E_AHI_TIMER_1,0,350); //off PWM1 vAHI_TimerStartRepeat(E_AHI_TIMER_2,si16_4-511,350); //on PWM2 } else { vAHI_TimerStartRepeat(E_AHI_TIMER_1,0,350); //off PWM1 vAHI_TimerStartRepeat(E_AHI_TIMER_2,0,350); //off PWM2 } //ch2（マグネットアクチュエーター駆動） if(si16_5 < 511) { vAHI_TimerStartRepeat(E_AHI_TIMER_4,0,350); //off PWM4 vAHI_TimerStartRepeat(E_AHI_TIMER_3,511-si16_5,350); //on PWM3 } else if(511 < si16_5) { vAHI_TimerStartRepeat(E_AHI_TIMER_3,0,350); //off PWM3 vAHI_TimerStartRepeat(E_AHI_TIMER_4,si16_5-511,350); //on PWM4 } else { vAHI_TimerStartRepeat(E_AHI_TIMER_3,0,350); //off PWM3 vAHI_TimerStartRepeat(E_AHI_TIMER_4,0,350); //off PWM4 } } else { //サーボ出力の場合 if((u32AHI_DioReadInput() & 0x00000020) == 0x00000020) //DIO5 High:ノーマル出力 Low:ミキシング出力 { //サーボ（ノーマル）の場合 switch(mode_ctr[0] & 0x03) { case 0: si16_4 = get_pulse(ch1i); //パルス幅に変換（ノーマル） si16_5 = get_pulse(ch2i); //パルス幅に変換（ノーマル） break; case 1: si16_4 = 1500-(get_pulse(ch1i)-1500); //パルス幅に変換（リバース） si16_5 = get_pulse(ch2i); //パルス幅に変換（ノーマル） break; case 2: si16_4 = get_pulse(ch1i); //パルス幅に変換（ノーマル） si16_5 = 1500-(get_pulse(ch2i)-1500); //パルス幅に変換（リバース） break; case 3: si16_4 = 1500-(get_pulse(ch1i)-1500); //パルス幅に変換（リバース） si16_5 = 1500-(get_pulse(ch2i)-1500); //パルス幅に変換（リバース） break; } } else { //サーボ（ミキシング）の場合 si16_2 = get_pulse(ch1i) - 1500; //CH1 サーボ用パルス幅に変換 si16_3 = get_pulse(ch2i) - 1500; //CH2 サーボ用パルス幅に変換 switch(mode_ctr[0] & 0x0f) { case 0: si16_4 = (3000 + (si16_2 + si16_3))/2; //ミキシング方向 si16_5 = (3000 + (si16_2 + si16_3))/2; //ミキシング方向 break; case 1: si16_4 = (3000 + (si16_2 + si16_3))/2; //ミキシング方向 si16_5 = (3000 + (-si16_2 + si16_3))/2; //ミキシング方向 break; case 2: si16_4 = (3000 + (si16_2 + si16_3))/2; //ミキシング方向 si16_5 = (3000 + (si16_2 - si16_3))/2; //ミキシング方向 break; case 3: si16_4 = (3000 + (si16_2 + si16_3))/2; //ミキシング方向 si16_5 = (3000 + (-si16_2 - si16_3))/2; //ミキシング方向 break; case 4: si16_4 = (3000 + (-si16_2 + si16_3))/2; //ミキシング方向 si16_5 = (3000 + (si16_2 + si16_3))/2; //ミキシング方向 break; case 5: si16_4 = (3000 + (-si16_2 + si16_3))/2; //ミキシング方向 si16_5 = (3000 + (-si16_2 + si16_3))/2; //ミキシング方向 break; case 6: si16_4 = (3000 + (-si16_2 + si16_3))/2; //ミキシング方向 si16_5 = (3000 + (si16_2 - si16_3))/2; //ミキシング方向 break; case 7: si16_4 = (3000 + (-si16_2 + si16_3))/2; //ミキシング方向 si16_5 = (3000 + (-si16_2 - si16_3))/2; //ミキシング方向 break; case 8: si16_4 = (3000 + (si16_2 - si16_3))/2; //ミキシング方向 si16_5 = (3000 + (si16_2 + si16_3))/2; //ミキシング方向 break; case 9: si16_4 = (3000 + (si16_2 - si16_3))/2; //ミキシング方向 si16_5 = (3000 + (-si16_2 + si16_3))/2; //ミキシング方向 break; case 10: si16_4 = (3000 + (si16_2 - si16_3))/2; //ミキシング方向 si16_5 = (3000 + (si16_2 - si16_3))/2; //ミキシング方向 break; case 11: si16_4 = (3000 + (si16_2 - si16_3))/2; //ミキシング方向 si16_5 = (3000 + (-si16_2 - si16_3))/2; //ミキシング方向 break; case 12: si16_4 = (3000 + (-si16_2 - si16_3))/2; //ミキシング方向 si16_5 = (3000 + (si16_2 + si16_3))/2; //ミキシング方向 break; case 13: si16_4 = (3000 + (-si16_2 - si16_3))/2; //ミキシング方向 si16_5 = (3000 + (-si16_2 + si16_3))/2; //ミキシング方向 break; case 14: si16_4 = (3000 + (-si16_2 - si16_3))/2; //ミキシング方向 si16_5 = (3000 + (si16_2 - si16_3))/2; //ミキシング方向 break; case 15: si16_4 = (3000 + (-si16_2 - si16_3))/2; //ミキシング方向 si16_5 = (3000 + (-si16_2 - si16_3))/2; //ミキシング方向 break; } } //ch1 （サーボ駆動） vAHI_TimerStartSingleShot(E_AHI_TIMER_1,si16_4,30000); // TimerName , on_time , period //ch2 （サーボ駆動） vAHI_TimerStartSingleShot(E_AHI_TIMER_3,si16_5,30000); // TimerName , on_time , period } //ch3 モーター駆動 //if(center == 1) if((u32AHI_DioReadInput() & 0x00004000) == 0x00004000) //DIO14 High:モーターオフポイントがスティック中央の場合 Low:モーターオフポイントがスティック下方の場合 { //モーターオフポイントがスティック中央の場合。 mpwm_period = 400; //PWM周期 mon_duty = ch3i - 600; //ON duty } else { //モーターオフポイントがスティック下方の場合。 mpwm_period = 800; //PWM周期 mon_duty = ch3i - 200; //ON duty } //デジタル出力処理 ui8_1=douti & 0x000000FF; //8bitデータに変換 digi_out(ui8_1); } //PWMによるモータ　ONフラグ if(0 < mon_duty) { mpwm_flag = 0x01; } else { mpwm_flag = 0x00; mon_duty = 0; } //バッテリー電圧低下検出 vAHI_AdcEnable(E_AHI_ADC_SINGLE_SHOT , E_AHI_AP_INPUT_RANGE_2 , E_AHI_ADC_SRC_ADC_1); // set AD channel vAHI_AdcStartSample(); // ADC開始 vWait(100); //70usec wait si16_3 = u16AHI_AdcRead(); // AD値の読み出し if(650 < si16_3) { if(mon_tmr == 0) { mbat_flag = 0x01; //モーターON フラグ　セット moff_tmr = 100; //モーターOFFタイマ　セット } else { mon_tmr--; } } else { if(moff_tmr == 0) { mbat_flag = 0x00; //モーターOFF フラグ　セット mon_tmr = 100; //モーターONタイマ　セット } else { moff_tmr--; } } //信号ロスト時の処理 rcv_tmr--; if(rcv_tmr == 0) { rcv_tmr = 200; } if(rcv_tmr < 190) { vAHI_DioSetOutput(0x00000008, 0x00000000); //off LED mlost_flag = 0x00; if((u32AHI_DioReadInput() & 0x00040000) == 0x00040000) //DIO18 High:マグネットアクチュエータ出力 Low:サーボ出力 { //マグネットアクチュエータ出力の場合 vAHI_TimerStartRepeat(E_AHI_TIMER_1,0,350); //off PWM1 vAHI_TimerStartRepeat(E_AHI_TIMER_2,0,350); //off PWM2 vAHI_TimerStartRepeat(E_AHI_TIMER_3,0,350); //off PWM3 vAHI_TimerStartRepeat(E_AHI_TIMER_4,0,350); //off PWM4 } } if(190 <= rcv_tmr) { vAHI_DioSetOutput(0x00000000, 0x00000008); //on LED } //drive motor if(mlost_flag & mbat_flag & mpwm_flag) { vAHI_TimerStartRepeat(E_AHI_TIMER_0,mon_duty,mpwm_period); //on motor } else { vAHI_TimerStartRepeat(E_AHI_TIMER_0,0,mpwm_period); //off motor } //モニタ出力用 mctr--; if(mctr == 0) //1sec間隔に間引いて実行 { mctr =50; //モニタ出力 vfPrintf(&sSerStream , "Hdr=%02X CH1=%03X CH2=%03X CH3=%03X CH4=%03X SW=%02X CSUM=%02X\r\n" , hdri , ch1i , ch2i , ch3i , ch4i , douti , csumi); } } return; } void vProcessEvCorePwr(tsEvent *pEv, teEvent eEvent, uint32 u32evarg) { return; } void cbToCoNet_vNwkEvent(teEvent eEvent, uint32 u32arg) { return; } // // 以下 ToCoNet 既定のイベントハンドラ群 // // 割り込み発生後に随時呼び出される void cbToCoNet_vMain(void) { return; } // パケット受信時 void cbToCoNet_vRxEvent(tsRxDataApp *pRx) { static uint32 u32SrcAddrPrev = 0; static uint8 u8seqPrev = 0xFF; // 前回と同一の送信元＋シーケンス番号のパケットなら受け流す if (pRx->u32SrcAddr == u32SrcAddrPrev && pRx->u8Seq == u8seqPrev) { return; } // ペイロードを切り出してデバッグ出力 //char buf[64]; int len = (pRx->u8Len < sizeof(rcv_buf)) ? pRx->u8Len : sizeof(rcv_buf)-1; memcpy(rcv_buf, pRx->auData, len); u32SrcAddrPrev = pRx->u32SrcAddr; u8seqPrev = pRx->u8Seq; vAHI_DioSetOutput(0x00000000, 0x00000008); //on LED（受信確認用) rcv_tmr = 250; //受信タイマ mlost_flag = 0x01; return; } // パケット送信完了時 void cbToCoNet_vTxEvent(uint8 u8CbId, uint8 bStatus) { return; } // ハードウェア割り込み発生後（遅延呼び出し） void cbToCoNet_vHwEvent(uint32 u32DeviceId, uint32 u32ItemBitmap) { return; } // ハードウェア割り込み発生時 uint8 cbToCoNet_u8HwInt(uint32 u32DeviceId, uint32 u32ItemBitmap) { return FALSE; } // コールドスタート時 void cbAppColdStart(bool_t bAfterAhiInit) { uint32 ui32; if (!bAfterAhiInit) { // 必要モジュール登録手続き ToCoNet_REG_MOD_ALL(); } else { // SPRINTF 初期化 SPRINTF_vInit128(); // ToCoNet パラメータ sToCoNet_AppContext.u32AppId = RX_ID; //APP_ID; sToCoNet_AppContext.u8Channel = RX_CH; //channel sToCoNet_AppContext.bRxOnIdle = TRUE; // アイドル時にも受信 sToCoNet_AppContext.u16TickHz = 50; //Tick timer 20msec u32Seq = 0; // ユーザ定義のイベントハンドラを登録 ToCoNet_Event_Register_State_Machine(vProcessEvCore); // ハードウェア初期化 // デバッグ出力用 vSerialInit(); ToCoNet_vDebugInit(&sSerStream); ToCoNet_vDebugLevel(0); // 使用ポートの設定 bAHI_DoEnableOutputs(FALSE); //output enable vAHI_DoSetDataOut(0x00 , 0x03); //set output data high , Low vAHI_DoSetPullup(0x03 , 0x00); //set do pullup enable , disable ui32=0x00000008; vAHI_DioSetOutput(ui32 , (ui32^0x000fffff)); //set dio out high , low ui32=0x000a3f58; vAHI_DioSetDirection((ui32^0x000fffff) , ui32); //set dio directio inputport , outputport ui32=0x0005c0a7; vAHI_DioSetPullup(ui32 , (ui32^0x000fffff)); //set dio pullup enable , disable //ADCの設定 if (!bAHI_APRegulatorEnabled()) { vAHI_ApConfigure(E_AHI_AP_REGULATOR_ENABLE, E_AHI_AP_INT_ENABLE, // DISABLE にするとアナログ部の電源断 E_AHI_AP_SAMPLE_2, // サンプル数 2,4,6,8 が選択可能 E_AHI_AP_CLOCKDIV_500KHZ, // 周波数 250K/500K/1M/2M E_AHI_AP_INTREF); while(!bAHI_APRegulatorEnabled()); // 安定するまで待つ } while(u16AHI_InitialiseEEP(mode_ctr) < 1); //EEPROM イニシャライズ vWait(1400); //1msec wait while(iAHI_ReadDataFromEEPROMsegment(0,0,mode_ctr,1) == 1); //モードカウンタの書き込み //PWMの設定 //pwm 0 （ch3 モーター駆動用） vAHI_TimerEnable(E_AHI_TIMER_0,4,FALSE,FALSE,TRUE); //tinerName , プリスケーラ(2^n) , timer_int(rise) , timer_int(down) ,pwm_mode vAHI_TimerDIOControl(E_AHI_TIMER_0,TRUE); //timerName , pwm_out vAHI_TimerConfigureOutputs(E_AHI_TIMER_0,TRUE,TRUE); //TimerName , inversion , ex_input （出力パルス：正） vAHI_TimerClockSelect(E_AHI_TIMER_0,FALSE,TRUE); // TimerName , ex_clock_sorce , clock_out_invert vAHI_TimerStartRepeat(E_AHI_TIMER_0,0,400); //off motor //pwm 1 (ch1用) vAHI_TimerEnable(E_AHI_TIMER_1,4,FALSE,FALSE,TRUE); //tinerName , プリスケーラ(2^n) , timer_int(rise) , timer_int(down) ,pwm_mode vAHI_TimerDIOControl(E_AHI_TIMER_1,TRUE); //timerName , pwm_out vAHI_TimerConfigureOutputs(E_AHI_TIMER_1,TRUE,TRUE); //出力パルス：正。　第２パラメータをFALSEにすると出力パルスは負になる。 vAHI_TimerClockSelect(E_AHI_TIMER_1,FALSE,TRUE); // TimerName , ex_clock_sorce , clock_out_invert //pwm 2 (ch1用) vAHI_TimerEnable(E_AHI_TIMER_2,4,FALSE,FALSE,TRUE); //tinerName , プリスケーラ(2^n) , timer_int(rise) , timer_int(down) ,pwm_mode vAHI_TimerDIOControl(E_AHI_TIMER_2,TRUE); //timerName , pwm_out vAHI_TimerConfigureOutputs(E_AHI_TIMER_2,TRUE,TRUE); //出力パルス：正。　第２パラメータをFALSEにすると出力パルスは負になる。 vAHI_TimerClockSelect(E_AHI_TIMER_2,FALSE,TRUE); // TimerName , ex_clock_sorce , clock_out_invert //pwm 3 (ch2用) vAHI_TimerEnable(E_AHI_TIMER_3,4,FALSE,FALSE,TRUE); //tinerName , プリスケーラ(2^n) , timer_int(rise) , timer_int(down) ,pwm_mode vAHI_TimerDIOControl(E_AHI_TIMER_3,TRUE); //timerName , pwm_out vAHI_TimerConfigureOutputs(E_AHI_TIMER_3,TRUE,TRUE); //出力パルス：正。　第２パラメータをFALSEにすると出力パルスは負になる。 vAHI_TimerClockSelect(E_AHI_TIMER_3,FALSE,TRUE); // TimerName , ex_clock_sorce , clock_out_invert //pwm 4 (ch2用) vAHI_TimerEnable(E_AHI_TIMER_4,4,FALSE,FALSE,TRUE); //tinerName , プリスケーラ(2^n) , timer_int(rise) , timer_int(down) ,pwm_mode vAHI_TimerDIOControl(E_AHI_TIMER_4,TRUE); //timerName , pwm_out vAHI_TimerConfigureOutputs(E_AHI_TIMER_4,TRUE,TRUE); //出力パルス：正。　第２パラメータをFALSEにすると出力パルスは負になる。 vAHI_TimerClockSelect(E_AHI_TIMER_4,FALSE,TRUE); // TimerName , ex_clock_sorce , clock_out_invert if((u32AHI_DioReadInput() & 0x00040000) == 0x00000000) //DIO18 High:マグネットアクチュエータ出力 Low:サーボ出力 { //サーボ出力の場合 vAHI_TimerDIOControl(E_AHI_TIMER_2,FALSE); //timerName , pwm_OFF vAHI_TimerDIOControl(E_AHI_TIMER_4,FALSE); //timerName , pwm_OFF vAHI_DioSetOutput(0x00000000 , 0x00001000); //set dio 12 vAHI_DioSetOutput(0x00000000 , 0x00020000); //set dio 17 } // MAC 層開始 ToCoNet_vMacStart(); } } // ウォームスタート時 void cbAppWarmStart(bool_t bAfterAhiInit) { return; } //以下　自作の関数 //void _set_mode(uint8 i) void _set_mode(void) { if(set_flag == 0) //セッティングモードか？（サーボリバース用） { if(767 < ch1i) //CH1の値が767以上か？ { set_tmr--; if(set_tmr == 0) //5秒経過したか？ { mode_ctr[0]++; //if(15 < mode_ctr[0]) //{ // mode_ctr[0] = 0; //} while(iAHI_WriteDataIntoEEPROMsegment(0,0,mode_ctr,1) == 1) //モードカウンタの書き込み vWait(1400); //1msec wait while(iAHI_ReadDataFromEEPROMsegment(0,0,mode_ctr,1) == 1); //モードカウンタの書き込み set_flag = 1; //セッティングモード終了 } } else { while(iAHI_ReadDataFromEEPROMsegment(0,0,mode_ctr,1) == 1); // if(15 < mode_ctr[0]) // { // mode_ctr[0] = 0; // } set_flag = 1; //セッティングモード終了 } } return; } //デジタル出力処理 void digi_out(uint8 digi_data) { //digital output 1ch if(digi_data & 0x01) { vAHI_DioSetOutput(0x00000200 , 0x00000000); //set dio 9 } else { vAHI_DioSetOutput(0x00000000 , 0x00000200); //clear dio 9 } //digital output 2ch if(digi_data & 0x02) { vAHI_DioSetOutput(0x00000100 , 0x00000000); //set dio 8 } else { vAHI_DioSetOutput(0x00000000 , 0x00000100); //clear dio 8 } // digital output 3ch if(digi_data & 0x04) { vAHI_DioSetOutput(0x00000010 , 0x00000000); //set dio 4 } else { vAHI_DioSetOutput(0x00000000 , 0x00000010); //clear dio 4 } //digital output 4ch if(digi_data & 0x08) { vAHI_DioSetOutput(0x00080000 , 0x00000000); //set dio 19 } else { vAHI_DioSetOutput(0x00000000 , 0x00080000); //clear dio 19 } //digital output 5ch // if(digi_data & 0x10) // { // vAHI_DioSetOutput(0x00040000 , 0x00000000); //set dio 18 // } // else // { // vAHI_DioSetOutput(0x00000000 , 0x00040000); //clear dio 18 // } // //digital output 6ch // if(digi_data & 0x20) // { // vAHI_DioSetOutput(0x00000020 , 0x00000000); //set dio 5 // } // else // { // vAHI_DioSetOutput(0x00000000 , 0x00000020); //clear dio 5 // } //digital output 7ch // if(digi_data & 0x40) // { // vAHI_DioSetOutput(0x00004000 , 0x00000000); //set dio 14 // } // else // { // vAHI_DioSetOutput(0x00000000 , 0x00004000); //clear dio 14 // } //digital output 8ch // if(digi_data & 0x80) // { // vAHI_DioSetOutput(0x00000004 , 0x00000000); //set dio 02 // } // else // { // vAHI_DioSetOutput(0x00000000 , 0x00000004); //clear dio 02 // } return; }