START. (coldstart) (start_eng) (current_fb) (stop_eng) (drive servo) 1A 5 6V Y. (start_eng) 46V. (charge) (emergency)

Transcription

1 START (coldstart) A (start_eg) (current_fb) (drive servo) (stop_eng) 1A 5 46V 6V 46V (start_eg) (charge) (emergency)

2 (coldstart) coldstart O O? OFF 500rpm? O 60 RETUR

3 (start_eg) Start_EG (drive servo) (coldstart) O A (drive servo) 500rpm? RETUR

4 (current_fb) Current_FB A RETUR

5 (charge) charge 10A (drive servo) 55V RETUR

6 (drive_servo) drive_servo RETUR (stop_eg) Stop_EG (drive servo) OFF RETUR

7 '{$STAMP BS2} '{$PBASIC 2.5} ' Prominence ' 4X-7 V1.0 T.Miyamura I VAR Word J VAR Byte ' A VAR Word B VAR Word ' VAR Word X VAR Word maxcurrent CO 1620 'maxcurrent=imax[a]* :10A maxvoltage CO 2145 ' 'maxvoltage=vmax[v]*39 :55V thon CO 725 ' thoff CO 1070 ' Irate CO $0093 :Irate=(thoff-thon)/600*256 Vrate CO $0026 ' :Vrate=(thoff-thon)/2340*256 tar VAR Word ' PAUSE 1000 GOSUB CLEAR LOX=0:LO=0:GOSUB LOCATE SEROUT display,boud,speed,["-genereter Monitor-"] LOX=0:LO=3:GOSUB LOCATE SEROUT display,boud,speed,["status:"] LOX=0:LO=1:GOSUB LOCATE SEROUT display,boud,speed,["e:"] LOX=0:LO=2:GOSUB LOCATE SEROUT display,boud,speed,["vb:"] LOX=5:LO=2:GOSUB LOCATE SEROUT display,boud,speed,["."] LOX=11:LO=1:GOSUB LOCATE SEROUT display,boud,speed,["gpwr:"] LOX=19:LO=1:GOSUB LOCATE SEROUT display,boud,speed,["w"] LOX=11:LO=2:GOSUB LOCATE SEROUT display,boud,speed,["idy:"] LOX=17:LO=2:GOSUB LOCATE SEROUT display,boud,speed,["."] LOX=19:LO=2:GOSUB LOCATE SEROUT display,boud,speed,["a"] LOX=7:LO=1:GOSUB LOCATE SEROUT display,boud,speed,["rpm"] LOX=8:LO=2:GOSUB LOCATE SEROUT display,boud,speed,["v"]

8 ' coldstart: HIGH ign I=0 IPUT 9 DO IF I9=1 THE HIGH selmot ELSE LOW selmot mcpdatabits=%1011 'P9: (push on 1) 'ADC mot IF V>135 THE IF I9=0 THE I=I+1 LOX=7:LO=3:GOSUB LOCATE SEROUT display,boud,speed,["warming Up..."] ELSE 'e>500rpm ' STT OFF ' I=0 'e>500rpm STT O LOX=7:LO=3:GOSUB LOCATE SEROUT display,boud,speed,["clanking... "] ELSE I=0 LOX=7:LO=3:GOSUB LOCATE SEROUT display,boud,speed,["start Engine."] 'e<500rpm STT OFF : LOOP UTIL(I>500) ' 1 Eng=1

9 ' main : mcpdatabits=%1001 IF V<1420 THE ' ' 5A IF EG=1 THE GOSUB stop_eng LOX=7:LO=3:GOSUB LOCATE SEROUT display,boud,speed,["standing By. "] GOSUB pprint mcpdatabits=%1010 IF V<1794 THE charge GOTO main GOSUB START_EG IF EG=0 THE coldstart ' '46V ' ' I=0 normal : GOSUB current_fb GOSUB drive_servo IF tar<1300 THE ' 1A 5sec I=I+1 ELSE I=0 IF I>18 THE GOSUB stop_eng GOTO main

10 mcpdatabits=%1010 IF V<1794 THE charge IF V>2184 THE ERRCODE=2 GOTO EMERGEC mcpdatabits=%1011 ' '46V '56V 'ADC mot 'e<500rpm IF V<135 THE ERRCODE=0 GOTO EMERGEC GOTO normal ' charge: GOSUB start_eng IF EG=0 THE coldstart GOSUB CC GOSUB drive_servo mcpdatabits=%1010 IF V>=maxvoltage THE main mcpdatabits=%1011 IF V<135 THE ERRCODE=0 GOTO EMERGEC GOTO charge '55V 'ADC mot 'e<500rpm

11 ' EG stop_eng : command=thoff ' FOR i=1 TO 4 GOSUB drive_servo EXT LOW ign 'IG OFF EG=0 RETUR ' CC CC : LOX=7:LO=3:GOSUB LOCATE SEROUT display,boud,speed,["mode CC "] tar=maxcurrent mcpdatabits=%1000 IF tar>v THE A=tar-V B=A*/Irate command=command-b ' ELSE A=V-tar B=a*/Irate command=command+b RETUR

12 ' ' current_fb : : 'A : B :command LOX=7:LO=3:GOSUB LOCATE SEROUT display,boud,speed,["mode CFB "] mcpdatabits=%1001 'DCCT-2 ' ' 1A tar=v+40 tar=tar MAX maxcurrent mcpdatabits=%1000 =V IF tar> THE A=tar- B=A*/Irate command=command-b ELSE A=-tar B=A*/Irate command=command+b RETUR 'DCCT-1 ' 'command ' ) 'command ) ' ' ign CO 7 pin7 selmot CO 8 pin 8 EG VAR Bit 'EG 0: 1: START_EG : IF EG=1 THE EDSTART ' HIGH ign 'IG O LOX=7:LO=3:GOSUB LOCATE SEROUT display,boud,speed,["starting EG."] command=thon FOR i=1 TO 4 GOSUB drive_servo EXT '

13 HIGH selmot mcpdatabits=%1000 FOR I=1 TO 560 IF V>1220 THE EXIT EXT LOW selmot command=thoff ' 'ADC DCCT-1 ' : 5sec '(ADC 8.8ms ' > ) ' ' FOR i=1 TO 4 GOSUB drive_servo EXT mcpdatabits=%1011 'ADC mot ' IF V>135 THE LOX=7:LO=3:GOSUB LOCATE SEROUT display,boud,speed,["eg Started. "] EG=1 PAUSE 500 EDSTART: RETUR

14 ' MCP3208(12Bit 8ch A-D ' ) MCP3208 ' pin_bs_data_in CO 4 ' ' pin 4 pin_bs_data_out CO 5 pin 5 pin_mcp_clock CO 3 ' pin 3 pin_mcp_chip_select CO 6 'mcp pin 6 mcpdatabits VAR ib ch ADCERROR V VAR Bit VAR Word 'mcpdatabits=%1000' 3bit: ' 1bit:1= 0= GETADC: J=0 DO V =$0000 B =$0000 LOW pin_mcp_clock HIGH pin_mcp_chip_select LOW pin_mcp_chip_select SHIFTOUT pin_bs_data_out,pin_mcp_clock,msbfirst,[%1 1,mcpDataBits 4] ' ch SHIFTI pin_bs_data_in, pin_mcp_clock,msbpre, [V 14] SHIFTI pin_bs_data_in, pin_mcp_clock,lsbpre, [B 11] V=V & % B =B << 1 B =(V & % ) B IF V<>B THE ADCERROR=1 ELSE ADCERROR=0 HIGH pin_mcp_chip_select J=J+1 IF J=10 THE ERRCODE=1 GOTO EMERGEC LOOP UTIL(ADCERROR=0) RETUR

15 ' drive_servo ' ' servo1 CO 2 pin2 command VAR Word old_command VAR Word drive_servo: A=ABS(command-old_command) ' command 5LSB SELECT A CASE 0 TO 2 command=old_command EDSELECT command = command MI thon command = command MAX thoff old_command=command PAUSE 10 GOSUB pprint RETUR '

16 ' 'LCD display CO 0 'LCD pin0 boud ' ' speed CO 5 ( ) CO (9600boud) ' ' LOX VAR Byte X (0-19) LO VAR ib ' (0-3) LOCATE: SEROUT display,boud,speed,[$0c,lox,lo] RETUR C_RETUR : ' SEROUT display,boud,speed,[$0d] RETUR CLEAR : ' SEROUT display,boud,speed,[$09] RETUR ' ERRCODE VAR ib EMERGEC: LOW ign LOW selmot GOSUB CLEAR SEROUT display,boud,speed,["geeretor HALTED."] GOSUB C_RETUR SEROUT display,boud,speed,["check SSTEM."] GOSUB C_RETUR SEROUT display,boud,speed,["code:",dec1 ERRCODE] GOSUB C_RETUR ED

17 ' PPRIT: ' mcpdatabits=%1011 'ADC mot =V*/$03B3 = MAX 9999 LOX=3:LO=1:GOSUB LOCATE SEROUT display,boud,speed,[dec4 ] ' mcpdatabits=%1010 =V/39 X=(V*/$0042)-(*10) IF X>=10 THE =+1 LOX=3:LO=2:GOSUB LOCATE SEROUT display,boud,speed,[dec2 ] ' LOX=6:LO=2:GOSUB LOCATE SEROUT display,boud,speed,[dec1 X] ' mcpdatabits=%1000 V=V MI 1220 =(V-1220 )/4 'DCCT-1

18 mcpdatabits=%1010 X=V/39 A=(V*/$0042)-(X*10) ' IF A>=5 THE X=X+1 X=(X*+5)/10 LOX=16:LO=1:GOSUB LOCATE SEROUT display,boud,speed,[dec3 X] ' mcpdatabits=%1000 V=V MI 1220 =(V-1220 )/40 X=(V-1220)/4-(*10) IF X>=10 THE =+1 LOX=15:LO=2:GOSUB LOCATE SEROUT display,boud,speed,[dec2 ] LOX=18:LO=2:GOSUB LOCATE SEROUT display,boud,speed,[dec1 X] RETUR 'DCCT-1