kuchi_robo.c - TeraPad

Size: px

Start display at page:

Download "kuchi_robo.c - TeraPad"

いつやたけくま
5 years ago
Views:

1 #include <PIC.h> #include <Delays.h> #include <maths.h> DELA # config 0x3F50 コンフィギュレーションビットの設定 62 bit4=0( コードプロテクト =OFF),bit3=0( パワーアップタイマ =OFF) bit2=0( ウォッチドッグタイマ =OFF),bit1,0='1','0'(HS 発振 ) 00=LP,01=XT,10=HS,11=RC オシレター #define ADC_START ADCON0 =(1<<GO) AD On #define TST_ADC_NOT_DONE (ADCON0&(1<<GO)) AD Off #define duty_center 412 #define PORTBIT(adr, bit) ((unsigned)(&adr)*8+(bit)) static bit PORTBIT(PORTB, 2); static bit PORTBIT(PORTB, 1); static bit PORTBIT(PORTB, 0); static bit PORTBIT(PORTB, 3); static bit PORTBIT(PORTB, 4); static bit PORTBIT(PORTB, 5); static bit PORTBIT(PORTB, 6); static bit PORTBIT(PORTB, 7); static bit PORTBIT(PORTA, 0); static bit PORTBIT(PORTA, 1); static bit PORTBIT(PORTA, 2); static bit PORTBIT(PORTA, 3); static bit PORTBIT(PORTA, 4); static bit PORTBIT(PORTA, 6); static bit PORTBIT(PORTA, 7); void main(void); void io_init(void); Initialise display to I/O int ADConversion(unsigned char); unsigned int measurement(unsigned short); void pwm(unsigned char); void set_duty(unsigned int); void motordrv(unsigned char, unsigned char, unsigned char, unsigned char ); void wait_min(unsigned char); void kubi_furi(unsigned char); void LED_drv(unsigned char,unsigned char, unsigned char, unsigned char, unsigned int ); unsigned char kubifuri(unsigned char); void udefuru(unsigned char); unsigned char kyori(void); unsigned char jintai(void); unsigned int rand_cal(unsigned char); Main program void main(void) - 1 -

2 Temperature humidity static unsigned char lp, lop, lr_f, kabe_fg; static unsigned int kabe_tm1, kabe_tm2,no_move_cnu, rnd_fg; io_init(); pwm(0xf4); set_duty(160); AMU_MOTOR=1; RLED=0; LLED=0; AI_RIGHT=1; AI_LEFT=1; KYORI_PW=1; PWM=On:1/1 180 / duty Center=412 aum motor off mentama LED off mentama LED off kubifuri motor R_off kubifuri motor L_off kyori sens pw off Stert Key input do LED_drv(1, 1, 0, 0, 50); while(key==1); RLED=1; LED on LLED=1; LED on kubi furi LED_drv(0, 0, 0, 0, 0); LED ON kubifuri(1); ude kuchi mode udefuru(4); while(1) kabe_fg=0; LED_drv(1, 1, 1, 1, 0); if(jintai()!=0) jmp1: motordrv(0, 1, 1, 0); motor st ---Move time loop ---- for(lp=0;lp<70;lp++) zensin time loop if(kyori()!=0) kyor chak kabe_fg=1; motordrv(0, 0, 0, 0); motor sp goto jmp; jmp: lr_f=0; motordrv(0, 0, 0, 0); motor sp LED_drv(0, 0, 0, 0, 0); LED off if(kabe_fg!=0) lr_f=1; kabe_tm1=4; kabe_tm2=1000; else lr_f=kubifuri(1); udefuru(4); - 2 -

3 kabe_tm1=2; kabe_tm2=600; no_move_cnu=0; 5Min Loop fg clear jintai right/left houkou chak if(lr_f==1) motordrv(1, 0, 0, 1); motor bak wait_min(kabe_tm1); motordrv(0, 0, 0, 0); motor bak move time motor sp motordrv(0, 1, 0, 1); motor L Wait(kabe_tm2); motor L move time motordrv(0, 0, 0, 0); motor sp lr_f=0; wait_min(1); meka jintai sena wait time if(lr_f==2) motordrv(1, 0, 0, 1); motor bak wait_min(kabe_tm1); motor bak move time motordrv(0, 0, 0, 0); motor sp motordrv(1, 0, 1, 0); motor R Wait(kabe_tm2); motor R move time motordrv(0, 0, 0, 0); motor sp lr_f=0; wait_min(1); meka jintai sena wait time no moushon move time 9min----- if(no_move_cnu++>180) 9Min no_move_cnu=0; 9Min Loop fg clear LED_drv(1, 1, 0, 0, 200); LED_drv(1, 1, 1, 1, 0); udefuru(5); /* do AMU_MOTOR=0; ON for(lp=0;lp<20;lp++) zensin time loop if(jintai()!=0) lop=99; kyor chak AMU_MOTOR=1; off while(lop++>3) */ kabe_tm1=1; kabe_tm2=600; lr_f=kubifuri(1); no moushon LED move time 120S----- if((no_move_cnu%40)==0) lop=0; kabe_fg=0; LED_drv(1, 0, 0, 1, 50); LED off rnd_fg=rand_cal(2); if(rnd_fg<6) do AMU_MOTOR=0; for(lp=0;lp<rnd_fg;lp++) ON zensin time loop - 3 -

4 if(jintai()!=0)amu_motor=1; goto jmp1; AMU_MOTOR=1; off while(lop++>3) kyor chak Randam unsigned int rand_cal(unsigned char select) unsigned int ca; switch(select) case 1: vre_lop: ca=rand()%400+1; rand if (ca>400) ca=ca%400+1; if (ca<160) goto vre_lop; case 2: ca=(rand()%10)+1; rand 0-10 if(ca>10) ca=ca%10+1; return ca; jintai chaku unsigned char jintai(void) unsigned char yes_ng, lp; WORD ad; yes_ng=0; for(lp=0;lp<15;lp++) do ad=measurement(0); while(ad<150); Wait(100); min=10 max=20 Low lebl chak to 15 Loop (0.7V<ad) ad=measurement(0); if(ad>900) yes_ng=1;led_drv(1, 1, 0, 0, 200); jintai chak OK return yes_ng; - 4 -

5 kyori chaku unsigned char kyori(void) unsigned char yes_ng; WORD ad; yes_ng=0; KYORI_PW=0; kyori pwr on Wait(100); ad=measurement(1); if(ad>300)yes_ng=1;led_drv(1, 0, 0, 1, 200); kyori chak OK 1.45V KYORI_PW=1; kyori off return yes_ng; Ude furu Time 1=1S void udefuru(unsigned char tim) AMU_MOTOR=0; wait_min(tim); AMU_MOTOR=1; Kubifuri Time (S=0 L=1 R=2) unsigned char unsigned char lop,l_rf; ON off kubifuri(unsigned char kaisu) l_rf=0; for(lop=0;lop<kaisu;lop++) kubi_furi(0); L kaiten LED_drv(1, 1, 1, 1, 0); LED on wait_min(1); meka jintai sena wait time if(jintai()!=0)l_rf=1; kubi_furi(1); LED_drv(0, 0, 0, 0, 0); LED off kubi_furi(1); R kaiten LED_drv(1, 1, 1, 1, 0); LED on wait_min(1); meka jintai sena wait time if(jintai()!=0)l_rf=2; kubi_furi(2); LED_drv(0, 0, 0, 0, 0); LED off return l_rf; LED R=1,0,1,0 L=0,1,0,1 R.F=1,0,0,0 L.F=0,1,0,0-5 -

6 void LED_drv(unsigned char right1,unsigned char left1, unsigned char right2, unsigned char left2, unsigned int pause) unsigned char lp; for(lp=0;lp<4;lp++) RLED=right1; LLED=left1; Wait(pause); RLED=right2; LLED=left2; Wait(pause); 首振り 0=Right 1=left 2=senta void kubi_furi(unsigned char channel) switch(channel) case 0: AI_RIGHT=0; R/on Wait(1000); AI_RIGHT=1; R/off case 1: AI_LEFT=0; L/on Wait(800); AI_LEFT=1; L/off case 2: AI_RIGHT=0; R/on do RLED=1; LLED=1; while(ai_ichi!=1); AI_RIGHT=1; AI_motor R_off AI_LEFT=1; AI_motor L_off MOTOR DIVE IN1 IN2 PWM MODE (IC TB6552FN) H H H/L Short brake L H H/L cw/ccw H L H/L ccw/cw L L H/L stop H/L H/L H/L standby - 6 -

7 void motordrv(unsigned char inr_1, unsigned char inr_2, unsigned char inl_1, unsigned char inl_2) MOTOR_IN1R=inr_1;MOTOR_IN2R=inr_2; MOTOR_IN1L=inl_1;MOTOR_IN2L=inl_2; Wait one Time min 1=1S Max(255) void wait_min(unsigned char tm) unsigned char c; for(c=0;c<tm;c++) Wait(1000); A/D 変換 channel No / 20 Loop unsigned int measurement(unsigned short channel) unsigned int ad, cnt; ad = 0; for (cnt = 0; cnt < 20; cnt++) ad += ADConversion(channel); return (ad/20); A/D 変換 Set(0-6) 0=RA0,1=RA1,2=RA2,3=RA3,4=RA4,5=RB6,6=RB7(AN0:6) int ADConversion(unsigned char channel) WORD adval; channel&=0x0f; ADCON0=0xC1 (channel<<3); A/D module RC oscillator),a/d On truncate channel to 4 bits 0xC1=FRC (clock derived from the internal ADC_START; AD 変換スタート while(tst_adc_not_done) adval = (WORD)ADRESH << 8; adval = adval ADRESL; AD 変換ストップ upper8ビットを読み込む lower8ビットを読み込んで16ビットに合成する return adval; 初期処理この順番でないと動作しない - 7 -

8 void io_init(void) A/Dコンバータのリファレンス初期設定 ADCON1=0x80; 左詰,0x80=VRef+:VDD(5v) VRef-:VSS / 0xA0=VRef+:VDD(2.5-5v) VRef-:VSS I/O Mode Analog=1 Digital=0 Bit 0:7 (16F88 Onley アナログ入力の設定 ) ANSEL=0x3; RA0,1のみアナログ入力他はデジタル (X ) F88 内蔵オシレータの設定 ---- OSCCON=0x60; 内蔵オシレータの周波数を4MHzに設定 / 0x70=8MHz Comparator CMCON=0x7; Comparators Off PORTA=0; ポート (A)CLEAR PORTB=0; ポート (B)CLEAR TRISA=0x83; A ポート TRISB=0x20; B ポート PWM 周期設定 PWM 周期 = [(PR2) + 1] * 4 * 0.125uS(8MHz)or[0.25uS(4MHz)] *(TMR2 プリースケラー値 ) [49+1]*4*0.125*1=25uS 1/25uS = *1000*1000=40,000Hz PWM 周期数は 1/[PWM 周期 ] と定義されます On=0xf4 Off= void pwm(unsigned char tsw) PR2=199; CCP タイマー 2 周期レジスタにデータを書く (5KHz) 199 T2CON=tsw; 1:1,1:4,1:16 タイマープリースケラタイマー =ON Pwm_Change_DutyEx duty には 0 から 1024 の値を入れる / duty_center(412) CCP1CON CCPR1L にデユーテイサイクルを指定する PWM デユーテイサイクル =(CCPR1L:CCP1CON<5:4>)*Tosc*(TMR2 プリースケラー ) void set_duty(unsigned int duty) unsigned int temp1; unsigned char temp2; temp1 = duty; temp1 >>= 2; temp2 = temp1 & 0xff; CCPR1L = temp2; temp1 = duty; - 8 -

9 temp1 <<= 4; temp2 = temp1 0x0c; CCP1CON = temp2; <Motor IC TB6552FN Robt Kuchpaku Proguram> 2009/10/28> コンパイラー :FED PIC Compiler Version 基板性能センサー接続数 : アナログ (IN)=4ポート [RA0,RA1,RA2,RA3] (0-4092) I/O : デジタル (OUT)=1ポート [RA6] 補助ポート (in/out)=2ポート[ra4,rb5] CPU :16F88 (4MHz)4MHでないと動作しない電圧範囲モーター電圧 :4.3V :4.8-6V ピンアサイン PIC16F8X 液晶モジュール () 内はピン番号 RA Analog 1 / 赤外線センサー RA Analog 2 / 距離センサー RA Analog 3 / 目玉モーター右回転 RA Analog 4 / 目玉モーター左回転 RA I/O / 距離センサー電源制御 RA I/O in only RA I/O デジタル / 腕口モーター RA I/O / Key inp RB I/O / PWM RB I/O / モーター出力右 RB I/O / RB I/O / モーター出力左 RB I/O / RB I/O / 目玉モーター位置検出 RB I/O / LED 出力右目玉 RB I/O / LED 出力左目玉 PIC16F88 用にコーディングしてありますが一部改造すればそれ以外のデバイスでも使用可能です - 9 -

スライド 1

スライド 1 8. ステッピングモータの制御を学ぼう秋月電子通商 PIC ステッピングモータドライバキット ( 小型モータ付き ) を参照しました. 回路製作の詳細は第 0 章を参照してください. 1 2 第 0 章図 28 より完成写真 ( マイコン回路 + ステッピングモータ駆動回路 ) PIC マイコンによるステッピングモータの制御 PIC16F84 R 1 R 2 RB6 RB0 ステッピングモータ S