第2章繪圖儀項(xiàng)目設(shè)計(jì)

第2章繪圖儀項(xiàng)目設(shè)計(jì)2.2.1主程序模塊相關(guān)代碼//library.h#ifndeflibrary_h#definelibrary_h//標(biāo)準(zhǔn)庫(kù)#include<avr/io.h>#include<avr/pgmspace.h>#include<avr/interrupt.h>#include<avr/wdt.h>#include<util/delay.h>#include<math.h>#include<inttypes.h>#include<string.h>#include<stdlib.h>#include<stdint.h>#include<stdbool.h>//模塊庫(kù)#include"config.h"http://設(shè)置相關(guān)#include"shared.h"http://共享的定義、變量、函數(shù)#include"settings.h"http://參數(shù)相關(guān)設(shè)置#include"defaults.h"http://默認(rèn)設(shè)置#include"cpu_map.h"http://CPU映射#include"eeprom.h"http://EEPROM內(nèi)容存儲(chǔ)#include"gcode.h"http://GCode解讀代碼#include"motion_control.h"http://運(yùn)動(dòng)控制#include"planner.h"http://指令隊(duì)列緩存#include"print.h"http://格式化輸出字符串#include"probe.h"http://系統(tǒng)狀態(tài)探測(cè)#include"protocol.h"http://控制執(zhí)行協(xié)議#include"serial.h"http://發(fā)送/接收串口通信#include"spindle_control.h"http://主軸控制模塊#include"stepper.h"http://步進(jìn)電機(jī)驅(qū)動(dòng)#endif//main.c#include"library.h"http://系統(tǒng)全局變量結(jié)構(gòu)system_tsys;intmain(void){//初始化系統(tǒng)serial_init();//設(shè)置波特率和中斷settings_init();//從內(nèi)存中加載設(shè)置stepper_init();//配置步進(jìn)電機(jī)和中斷計(jì)時(shí)器system_init();//配置引腳memset(&sys,0,sizeof(system_t));sys.abort=true;sei();//中斷//加電后初始化循環(huán)for(;;){serial_reset_read_buffer();gc_init();spindle_init();probe_init();plan_reset();st_reset();//同步系統(tǒng)位置plan_sync_position();gc_sync_position();//重置系統(tǒng)變量sys.abort=false;sys_rt_exec_state=0;sys_rt_exec_alarm=0;sys.suspend=false;//主循環(huán)protocol_main_loop();}return0;}#ifndefcpu_map_h#definecpu_map_h#definePLATFORM"Atmega328p"#defineSERIAL_RXUSART_RX_vect#defineSERIAL_UDREUSART_UDRE_vect#defineSTEP_DDRDDRD#defineSTEP_PORTPORTD#defineX_STEP_BIT2#defineY_STEP_BIT3#defineSTEP_MASK((1<<X_STEP_BIT)|(1<<Y_STEP_BIT))#defineDIRECTION_DDRDDRD#defineDIRECTION_PORTPORTD#defineX_DIRECTION_BIT5#defineY_DIRECTION_BIT6#defineDIRECTION_MASK((1<<X_DIRECTION_BIT)|(1<<Y_DIRECTION_BIT))#defineSTEPPERS_DISABLE_DDRDDRB#defineSTEPPERS_DISABLE_PORTPORTB#defineSTEPPERS_DISABLE_BIT0#defineSTEPPERS_DISABLE_MASK(1<<STEPPERS_DISABLE_BIT)#defineSPINDLE_ENABLE_DDRDDRB#defineSPINDLE_ENABLE_PORTPORTB#ifdefVARIABLE_SPINDLE#ifdefUSE_SPINDLE_DIR_AS_ENABLE_PIN#defineSPINDLE_ENABLE_BIT5#else#defineSPINDLE_ENABLE_BIT3#endif#else#defineSPINDLE_ENABLE_BIT4#endif#ifndefUSE_SPINDLE_DIR_AS_ENABLE_PIN#defineSPINDLE_DIRECTION_DDRDDRB#defineSPINDLE_DIRECTION_PORTPORTB#defineSPINDLE_DIRECTION_BIT5#endif//定義輸入引腳#defineCONTROL_DDRDDRC#defineCONTROL_PINPINC#defineCONTROL_PORTPORTC#defineRESET_BIT0//引腳A0#defineFEED_HOLD_BIT1//引腳A1#defineCYCLE_START_BIT2//引腳A2#defineCONTROL_INTPCIE1#defineCONTROL_INT_vectPCINT1_vect#defineCONTROL_PCMSKPCMSK1#defineCONTROL_MASK((1<<RESET_BIT)|(1<<FEED_HOLD_BIT)|(1<<CYCLE_START_BIT))#defineCONTROL_INVERT_MASKCONTROL_MASK#definePROBE_DDRDDRC#definePROBE_PINPINC#definePROBE_PORTPORTC#definePROBE_BIT5#definePROBE_MASK(1<<PROBE_BIT)#ifdefVARIABLE_SPINDLE#definePWM_MAX_VALUE255.0#defineTCCRA_REGISTERTCCR2A#defineTCCRB_REGISTERTCCR2B#defineOCR_REGISTEROCR2A#defineCOMB_BITCOM2A1#defineWAVE0_REGISTERWGM20#defineWAVE1_REGISTERWGM21#defineWAVE2_REGISTERWGM22#defineWAVE3_REGISTERWGM23#defineSPINDLE_PWM_DDRDDRB#defineSPINDLE_PWM_PORTPORTB#defineSPINDLE_PWM_BIT3#endif#endif//shared.h#ifndefshared_h#defineshared_h#definefalse0#definetrue1//軸定義#defineN_AXIS2//軸數(shù)量#defineX_AXIS0#defineY_AXIS1//XY步進(jìn)電機(jī)定義#ifdefCOREXY#defineA_MOTORX_AXIS#defineB_MOTORY_AXIS#endif//定義參數(shù)#defineMM_PER_INCH(25.40)#defineINCH_PER_MM(0.0393701)#defineTICKS_PER_MICROSECOND(F_CPU/1000000)#defineclear_vector(a)memset(a,0,sizeof(a))#defineclear_vector_float(a)memset(a,0.0,sizeof(float)*N_AXIS)#defineclear_vector_long(a)memset(a,0.0,sizeof(long)*N_AXIS)#definemax(a,b)(((a)>(b))?(a):(b))#definemin(a,b)(((a)<(b))?(a):(b))#definebit(n)(1<<n)#definebit_true_atomic(x,mask){uint8_tsreg=SREG;cli();(x)|=(mask);SREG=sreg;}#definebit_false_atomic(x,mask){uint8_tsreg=SREG;cli();(x)&=~(mask);SREG=sreg;}#definebit_toggle_atomic(x,mask){uint8_tsreg=SREG;cli();(x)^=(mask);SREG=sreg;}#definebit_true(x,mask)(x)|=(mask)#definebit_false(x,mask)(x)&=~(mask)#definebit_istrue(x,mask)((x&mask)!=0)#definebit_isfalse(x,mask)((x&mask)==0)uint8_tread_float(char*line,uint8_t*char_counter,float*float_ptr);//延遲函數(shù)voiddelay_ms(uint16_tms);voiddelay_us(uint32_tus);floathypot_f(floatx,floaty);#endif//shared.c#include"library.h"#defineMAX_INT_DIGITS8//最大位數(shù)uint8_tread_float(char*line,uint8_t*char_counter,float*float_ptr){char*ptr=line+*char_counter;unsignedcharc;//找到第一個(gè)字符用指針指向它c(diǎn)=*ptr++;boolisnegative=false;if(c=='-'){isnegative=true;c=*ptr++;}elseif(c=='+'){c=*ptr++;}uint32_tintval=0;int8_texp=0;uint8_tndigit=0;boolisdecimal=false;while(1){c-='0';if(c<=9){ndigit++;if(ndigit<=MAX_INT_DIGITS){if(isdecimal){exp--;}intval=(((intval<<2)+intval)<<1)+c;}else{if(!(isdecimal)){exp++;}//丟棄溢出字符}}elseif(c==(('.'-'0')&0xff)&&!(isdecimal)){isdecimal=true;}else{break;}c=*ptr++;}//如果沒(méi)有數(shù)據(jù)就返回if(!ndigit){return(false);};//整形轉(zhuǎn)浮點(diǎn)floatfval;fval=(float)intval;if(fval!=0){while(exp<=-2){fval*=0.01;exp+=2;}if(exp<0){fval*=0.1;}elseif(exp>0){do{fval*=10.0;}while(--exp>0);}}if(isnegative){*float_ptr=-fval;}else{*float_ptr=fval;}*char_counter=ptr-line-1;return(true);}voiddelay_ms(uint16_tms){while(ms--){_delay_ms(1);}}voiddelay_us(uint32_tus){while(us){if(us<10){_delay_us(1);us--;}elseif(us<100){_delay_us(10);us-=10;}elseif(us<1000){_delay_us(100);us-=100;}else{_delay_ms(1);us-=1000;}}}//斜邊計(jì)算函數(shù).floathypot_f(floatx,floaty){return(sqrt(x*x+y*y));}2.2.2串口通信模塊相關(guān)代碼//serial.h#ifndefserial_h#defineserial_h#ifndefRX_BUFFER_SIZE#defineRX_BUFFER_SIZE128#endif#ifndefTX_BUFFER_SIZE#defineTX_BUFFER_SIZE64#endif#defineSERIAL_NO_DATA0xff#ifdefENABLE_XONXOFF#defineRX_BUFFER_FULL96#defineRX_BUFFER_LOW64#defineSEND_XOFF1#defineSEND_XON2#defineXOFF_SENT3#defineXON_SENT4#defineXOFF_CHAR0x13#defineXON_CHAR0x11#endifvoidserial_init();//寫1byte數(shù)據(jù)到TXbuffer中voidserial_write(uint8_tdata);//讀取buffer中最前方的1byte數(shù)據(jù)uint8_tserial_read();//清空讀取buffervoidserial_reset_read_buffer();//返回?cái)?shù)據(jù)數(shù)量uint8_tserial_get_rx_buffer_count();//返回已完成數(shù)據(jù)量uint8_tserial_get_tx_buffer_count();#endif//serial.c#include"library.h"uint8_tserial_rx_buffer[RX_BUFFER_SIZE];uint8_tserial_rx_buffer_head=0;volatileuint8_tserial_rx_buffer_tail=0;uint8_tserial_tx_buffer[TX_BUFFER_SIZE];uint8_tserial_tx_buffer_head=0;volatileuint8_tserial_tx_buffer_tail=0;#ifdefENABLE_XONXOFFvolatileuint8_tflow_ctrl=XON_SENT;#endif//返回RXbuffer里的數(shù)據(jù)量uint8_tserial_get_rx_buffer_count()S{uint8_trtail=serial_rx_buffer_tail;if(serial_rx_buffer_head>=rtail){return(serial_rx_buffer_head-rtail);}return(RX_BUFFER_SIZE-(rtail-serial_rx_buffer_head));}//返回RXbuffer里完成的數(shù)據(jù)量uint8_tserial_get_tx_buffer_count(){uint8_tttail=serial_tx_buffer_tail;if(serial_tx_buffer_head>=ttail){return(serial_tx_buffer_head-ttail);}return(TX_BUFFER_SIZE-(ttail-serial_tx_buffer_head));}voidserial_init(){//設(shè)置波特率#ifBAUD_RATE<57600uint16_tUBRR0_value=((F_CPU/(8L*BAUD_RATE))-1)/2;UCSR0A&=~(1<<U2X0);#elseuint16_tUBRR0_value=((F_CPU/(4L*BAUD_RATE))-1)/2;UCSR0A|=(1<<U2X0);#endifUBRR0H=UBRR0_value>>8;UBRR0L=UBRR0_value;UCSR0B|=1<<RXEN0;UCSR0B|=1<<TXEN0;UCSR0B|=1<<RXCIE0;}voidserial_write(uint8_tdata){uint8_tnext_head=serial_tx_buffer_head+1;if(next_head==TX_BUFFER_SIZE){next_head=0;}while(next_head==serial_tx_buffer_tail){if(sys_rt_exec_state&EXEC_RESET){return;}}serial_tx_buffer[serial_tx_buffer_head]=data;serial_tx_buffer_head=next_head;UCSR0B|=(1<<UDRIE0);}ISR(SERIAL_UDRE){uint8_ttail=serial_tx_buffer_tail;#ifdefENABLE_XONXOFFif(flow_ctrl==SEND_XOFF){UDR0=XOFF_CHAR;flow_ctrl=XOFF_SENT;}elseif(flow_ctrl==SEND_XON){UDR0=XON_CHAR;flow_ctrl=XON_SENT;}else#endif{//發(fā)送數(shù)據(jù)UDR0=serial_tx_buffer[tail];tail++;if(tail==TX_BUFFER_SIZE){tail=0;}serial_tx_buffer_tail=tail;}if(tail==serial_tx_buffer_head){UCSR0B&=~(1<<UDRIE0);}}//讀取buffer里隊(duì)首數(shù)據(jù)uint8_tserial_read(){uint8_ttail=serial_rx_buffer_tail;if(serial_rx_buffer_head==tail){returnSERIAL_NO_DATA;}else{uint8_tdata=serial_rx_buffer[tail];tail++;if(tail==RX_BUFFER_SIZE){tail=0;}serial_rx_buffer_tail=tail;#ifdefENABLE_XONXOFFif((serial_get_rx_buffer_count()<RX_BUFFER_LOW)&&flow_ctrl==XOFF_SENT){flow_ctrl=SEND_XON;UCSR0B|=(1<<UDRIE0);//ForceTX}#endifreturndata;}}//中斷ISR(SERIAL_RX){uint8_tdata=UDR0;uint8_tnext_head;switch(data){caseCMD_STATUS_REPORT:bit_true_atomic(sys_rt_exec_state,EXEC_STATUS_REPORT);break;caseCMD_CYCLE_START:bit_true_atomic(sys_rt_exec_state,EXEC_CYCLE_START);break;caseCMD_FEED_HOLD:bit_true_atomic(sys_rt_exec_state,EXEC_FEED_HOLD);break;caseCMD_RESET:mc_reset();break;default:next_head=serial_rx_buffer_head+1;if(next_head==RX_BUFFER_SIZE){next_head=0;}if(next_head!=serial_rx_buffer_tail){serial_rx_buffer[serial_rx_buffer_head]=data;serial_rx_buffer_head=next_head;#ifdefENABLE_XONXOFFif((serial_get_rx_buffer_count()>=RX_BUFFER_FULL)&&flow_ctrl==XON_SENT){flow_ctrl=SEND_XOFF;UCSR0B|=(1<<UDRIE0);}#endif}}}//重置讀取buffervoidserial_reset_read_buffer(){serial_rx_buffer_tail=serial_rx_buffer_head;#ifdefENABLE_XONXOFFflow_ctrl=XON_SENT;#endif}2.2.3指令隊(duì)列模塊相關(guān)代碼//system.h#ifndefsystem_h#definesystem_h#include"library.h"http://定義系統(tǒng)映射圖#defineEXEC_STATUS_REPORTbit(0)//位掩碼00000001#defineEXEC_CYCLE_STARTbit(1)//位掩碼00000010#defineEXEC_CYCLE_STOPbit(2)//位掩碼00000100#defineEXEC_FEED_HOLDbit(3)//位掩碼00001000#defineEXEC_RESETbit(4)//位掩碼00010000#defineEXEC_SAFETY_DOORbit(5)//位掩碼00100000#defineEXEC_MOTION_CANCELbit(6)//位掩碼01000000#defineEXEC_CRITICAL_EVENTbit(0)//位掩碼00000001#defineEXEC_ALARM_ABORT_CYCLEbit(3)//位掩碼00001000#defineEXEC_ALARM_PROBE_FAILbit(4)//位掩碼00010000//定義系統(tǒng)狀態(tài)圖#defineSTATE_IDLE0#defineSTATE_ALARMbit(0)#defineSTATE_CHECK_MODEbit(1)#defineSTATE_HOMINGbit(2)#defineSTATE_CYCLEbit(3)#defineSTATE_HOLDbit(4)#defineSTATE_SAFETY_DOORbit(5)#defineSTATE_MOTION_CANCELbit(6)//定義系統(tǒng)暫停#defineSUSPEND_DISABLE0#defineSUSPEND_ENABLE_HOLDbit(0)#defineSUSPEND_ENABLE_READYbit(1)#defineSUSPEND_ENERGIZEbit(2)#defineSUSPEND_MOTION_CANCELbit(3)//系統(tǒng)全局變量typedefstruct{uint8_tabort;uint8_tstate;uint8_tsuspend;int32_tposition[N_AXIS];int32_tprobe_position[N_AXIS];uint8_tprobe_succeeded;}system_t;externsystem_tsys;volatileuint8_tsys_probe_state;volatileuint8_tsys_rt_exec_state;volatileuint8_tsys_rt_exec_alarm;//初始化系統(tǒng)voidsystem_init();uint8_tsystem_execute_line(char*line);voidsystem_execute_startup(char*line);floatsystem_convert_axis_steps_to_mpos(int32_t*steps,uint8_tidx);voidsystem_convert_array_steps_to_mpos(float*position,int32_t*steps);#ifdefCOREXYint32_tsystem_convert_corexy_to_x_axis_steps(int32_t*steps);int32_tsystem_convert_corexy_to_y_axis_steps(int32_t*steps);#endif#endif//system.c#include"library.h"voidsystem_init(){CONTROL_DDR&=~(CONTROL_MASK);//配置輸入引腳#ifdefDISABLE_CONTROL_PIN_PULL_UPCONTROL_PORT&=~(CONTROL_MASK);#elseCONTROL_PORT|=CONTROL_MASK;#endifCONTROL_PCMSK|=CONTROL_MASK;PCICR|=(1<<CONTROL_INT);//修改中斷}ISR(CONTROL_INT_vect){uint8_tpin=(CONTROL_PIN&CONTROL_MASK);#ifndefINVERT_ALL_CONTROL_PINSpin^=CONTROL_INVERT_MASK;#endif//任何控制指令輸入都可以觸發(fā)if(pin){if(bit_istrue(pin,bit(RESET_BIT))){mc_reset();}elseif(bit_istrue(pin,bit(CYCLE_START_BIT))){bit_true(sys_rt_exec_state,EXEC_CYCLE_START);}}}//執(zhí)行啟動(dòng)腳本voidsystem_execute_startup(char*line){uint8_tn;for(n=0;n<N_STARTUP_LINE;n++){if(!(settings_read_startup_line(n,line))){report_status_message(STATUS_SETTING_READ_FAIL);}else{if(line[0]!=0){printString(line);report_status_message(gc_execute_line(line));}}}}uint8_tsystem_execute_line(char*line){uint8_tchar_counter=1;uint8_thelper_var=0;floatparameter,value;switch(line[char_counter]){case0:report_grbl_help();break;case'$':case'G':case'C':case'X':if(line[(char_counter+1)]!=0){return(STATUS_INVALID_STATEMENT);}switch(line[char_counter]){case'$'://打印設(shè)置if(sys.state&(STATE_CYCLE|STATE_HOLD)){return(STATUS_IDLE_ERROR);}else{report_grbl_settings();}break;case'G'://打印GCode打印模式report_gcode_modes();break;case'C'://檢查GCode代碼模式if(sys.state==STATE_CHECK_MODE){mc_reset();report_feedback_message(MESSAGE_DISABLED);}else{if(sys.state){return(STATUS_IDLE_ERROR);}//Requiresnoalarmmode.sys.state=STATE_CHECK_MODE;report_feedback_message(MESSAGE_ENABLED);}break;case'X'://關(guān)閉警報(bào)鎖if(sys.state==STATE_ALARM){report_feedback_message(MESSAGE_ALARM_UNLOCK);sys.state=STATE_IDLE;}break;}break;default:if(!(sys.state==STATE_IDLE||sys.state==STATE_ALARM)){return(STATUS_IDLE_ERROR);}switch(line[char_counter]){case'#':if(line[++char_counter]!=0){return(STATUS_INVALID_STATEMENT);}else{report_ngc_parameters();}break;case'I'://打印信息if(line[++char_counter]==0){settings_read_build_info(line);report_build_info(line);}else{//存儲(chǔ)啟動(dòng)行if(line[char_counter++]!='='){return(STATUS_INVALID_STATEMENT);}helper_var=char_counter;do{line[char_counter-helper_var]=line[char_counter];}while(line[char_counter++]!=0);settings_store_build_info(line);}break;case'R'://恢復(fù)默認(rèn)設(shè)置if(line[++char_counter]!='S'){return(STATUS_INVALID_STATEMENT);}if(line[++char_counter]!='T'){return(STATUS_INVALID_STATEMENT);}if(line[++char_counter]!='='){return(STATUS_INVALID_STATEMENT);}if(line[char_counter+2]!=0){return(STATUS_INVALID_STATEMENT);}switch(line[++char_counter]){case'$':settings_restore(SETTINGS_RESTORE_DEFAULTS);break;case'#':settings_restore(SETTINGS_RESTORE_PARAMETERS);break;case'*':settings_restore(SETTINGS_RESTORE_ALL);break;default:return(STATUS_INVALID_STATEMENT);}report_feedback_message(MESSAGE_RESTORE_DEFAULTS);mc_reset();break;case'N':if(line[++char_counter]==0){for(helper_var=0;helper_var<N_STARTUP_LINE;helper_var++){if(!(settings_read_startup_line(helper_var,line))){report_status_message(STATUS_SETTING_READ_FAIL);}else{report_startup_line(helper_var,line);}}break;}else{if(sys.state!=STATE_IDLE){return(STATUS_IDLE_ERROR);}helper_var=true;}default:if(!read_float(line,&char_counter,¶meter)){return(STATUS_BAD_NUMBER_FORMAT);}if(line[char_counter++]!='='){return(STATUS_INVALID_STATEMENT);}if(helper_var){helper_var=char_counter;do{line[char_counter-helper_var]=line[char_counter];}while(line[char_counter++]!=0);helper_var=gc_execute_line(line);if(helper_var){return(helper_var);}else{helper_var=trunc(parameter);settings_store_startup_line(helper_var,line);}}else{if(!read_float(line,&char_counter,&value)){return(STATUS_BAD_NUMBER_FORMAT);}if((line[char_counter]!=0)||(parameter>255)){return(STATUS_INVALID_STATEMENT);}return(settings_store_global_setting((uint8_t)parameter,value));}}}return(STATUS_OK);}floatsystem_convert_axis_steps_to_mpos(int32_t*steps,uint8_tidx){floatpos;#ifdefCOREXYif(idx==X_AXIS){pos=(float)system_convert_corexy_to_x_axis_steps(steps)/settings.steps_per_mm[A_MOTOR];}elseif(idx==Y_AXIS){pos=(float)system_convert_corexy_to_y_axis_steps(steps)/settings.steps_per_mm[B_MOTOR];}else{pos=steps[idx]/settings.steps_per_mm[idx];}#elsepos=steps[idx]/settings.steps_per_mm[idx];#endifreturn(pos);}voidsystem_convert_array_steps_to_mpos(float*position,int32_t*steps){uint8_tidx;for(idx=0;idx<N_AXIS;idx++){position[idx]=system_convert_axis_steps_to_mpos(steps,idx);}return;}//計(jì)算X/Y軸運(yùn)動(dòng)#ifdefCOREXYint32_tsystem_convert_corexy_to_x_axis_steps(int32_t*steps){return((steps[A_MOTOR]+steps[B_MOTOR])/2);}int32_tsystem_convert_corexy_to_y_axis_steps(int32_t*steps){return((steps[A_MOTOR]-steps[B_MOTOR])/2);}#endif//protocol.h#ifndefprotocol_h#defineprotocol_h//讀取隊(duì)列大小#ifndefLINE_BUFFER_SIZE#defineLINE_BUFFER_SIZE80#endif//啟動(dòng)主循環(huán)voidprotocol_main_loop();//實(shí)時(shí)命令執(zhí)行voidprotocol_execute_realtime();//執(zhí)行自動(dòng)循環(huán)voidprotocol_auto_cycle_start();//同步隊(duì)列voidprotocol_buffer_synchronize();#endif//protocol.c#include"library.h"http://定義不同對(duì)話類型#defineCOMMENT_NONE0#defineCOMMENT_TYPE_PARENTHESES1#defineCOMMENT_TYPE_SEMICOLON2staticcharline[LINE_BUFFER_SIZE];//執(zhí)行行號(hào)//執(zhí)行一行staticvoidprotocol_execute_line(char*line){protocol_execute_realtime();if(sys.abort){return;}#ifdefREPORT_ECHO_LINE_RECEIVEDreport_echo_line_received(line);#endifif(line[0]==0){report_status_message(STATUS_OK);}elseif(line[0]=='$'){//'$'系統(tǒng)指令report_status_message(system_execute_line(line));}elseif(sys.state==STATE_ALARM){report_status_message(STATUS_ALARM_LOCK);}else{report_status_message(gc_execute_line(line));}}voidprotocol_main_loop(){//打印初始信息report_init_message();//警報(bào)if(sys.state==STATE_ALARM){report_feedback_message(MESSAGE_ALARM_LOCK);}else{system_execute_startup(line);}uint8_tcomment=COMMENT_NONE;uint8_tchar_counter=0;uint8_tc;for(;;){while((c=serial_read())!=SERIAL_NO_DATA){if((c=='\n')||(c=='\r')){//行結(jié)束的標(biāo)志line[char_counter]=0;//設(shè)置字符終止標(biāo)志protocol_execute_line(line);//執(zhí)行comment=COMMENT_NONE;char_counter=0;}else{if(comment!=COMMENT_NONE){//忽略無(wú)效對(duì)話if(c==')'){if(comment==COMMENT_TYPE_PARENTHESES){comment=COMMENT_NONE;}}}else{if(c<=''){}elseif(c=='/'){}elseif(c=='('){comment=COMMENT_TYPE_PARENTHESES;}elseif(c==';'){comment=COMMENT_TYPE_SEMICOLON;}elseif(char_counter>=(LINE_BUFFER_SIZE-1)){report_status_message(STATUS_OVERFLOW);comment=COMMENT_NONE;char_counter=0;}elseif(c>='a'&&c<='z'){//大小寫轉(zhuǎn)化line[char_counter++]=c-'a'+'A';}else{line[char_counter++]=c;}}}}protocol_auto_cycle_start(); //自動(dòng)循環(huán)protocol_execute_realtime();//實(shí)時(shí)執(zhí)行if(sys.abort){return;}}return;}voidprotocol_execute_realtime(){uint8_trt_exec;do{//系統(tǒng)暫停循環(huán)rt_exec=sys_rt_exec_alarm;if(rt_exec){sys.state=STATE_ALARM;if(rt_exec&EXEC_ALARM_HARD_LIMIT){report_alarm_message(ALARM_HARD_LIMIT_ERROR);}elseif(rt_exec&EXEC_ALARM_SOFT_LIMIT){report_alarm_message(ALARM_SOFT_LIMIT_ERROR);}elseif(rt_exec&EXEC_ALARM_ABORT_CYCLE){report_alarm_message(ALARM_ABORT_CYCLE);}elseif(rt_exec&EXEC_ALARM_PROBE_FAIL){report_alarm_message(ALARM_PROBE_FAIL);}if(rt_exec&EXEC_CRITICAL_EVENT){report_feedback_message(MESSAGE_CRITICAL_EVENT);bit_false_atomic(sys_rt_exec_state,EXEC_RESET);do{}while(bit_isfalse(sys_rt_exec_state,EXEC_RESET));}bit_false_atomic(sys_rt_exec_alarm,0xFF);//情況警報(bào)}//檢查并執(zhí)行實(shí)時(shí)命令rt_exec=sys_rt_exec_state;if(rt_exec){//系統(tǒng)終止if(rt_exec&EXEC_RESET){sys.abort=true;return;}//執(zhí)行并報(bào)告串口填充if(rt_exec&EXEC_STATUS_REPORT){report_realtime_status();bit_false_atomic(sys_rt_exec_state,EXEC_STATUS_REPORT);}//保持狀態(tài)if(rt_exec&(EXEC_MOTION_CANCEL|EXEC_FEED_HOLD)){//狀態(tài)檢查if((sys.state==STATE_IDLE)||(sys.state&(STATE_CYCLE|STATE_MOTION_CANCEL|STATE_HOLD))){if(sys.state==STATE_CYCLE){st_update_plan_block_parameters();//讓步進(jìn)電機(jī)模塊保持狀態(tài)sys.suspend=SUSPEND_ENABLE_HOLD;}if(sys.state==STATE_IDLE){sys.suspend=SUSPEND_ENABLE_READY;}if(rt_exec&EXEC_MOTION_CANCEL){if(sys.state==STATE_CYCLE){sys.state=STATE_MOTION_CANCEL;}sys.suspend|=SUSPEND_MOTION_CANCEL;}}bit_false_atomic(sys_rt_exec_state,(EXEC_MOTION_CANCEL|EXEC_FEED_HOLD));}//步進(jìn)電機(jī)中斷判斷if(rt_exec&EXEC_CYCLE_START){if(!(rt_exec&(EXEC_FEED_HOLD|EXEC_MOTION_CANCEL))){if((sys.state==STATE_IDLE)||((sys.state&(STATE_HOLD|STATE_MOTION_CANCEL))&&(sys.suspend&SUSPEND_ENABLE_READY))){if(sys.suspend&SUSPEND_ENERGIZE){//延遲主軸的命令if(gc_state.modal.spindle!=SPINDLE_DISABLE){spindle_set_state(gc_state.modal.spindle,gc_state.spindle_speed);}}if(plan_get_current_block()&&bit_isfalse(sys.suspend,SUSPEND_MOTION_CANCEL)){sys.state=STATE_CYCLE;st_prep_buffer();st_wake_up();}else{sys.state=STATE_IDLE;}sys.suspend=SUSPEND_DISABLE;}}bit_false_atomic(sys_rt_exec_state,EXEC_CYCLE_START);}//重新初始化if(rt_exec&EXEC_CYCLE_STOP){if(sys.state&(STATE_HOLD)&&!(sys.soft_limit)){if(sys.suspend&SUSPEND_ENERGIZE){spindle_stop();coolant_stop();}bit_true(sys.suspend,SUSPEND_ENABLE_READY);}else{sys.suspend=SUSPEND_DISABLE;sys.state=STATE_IDLE;}bit_false_atomic(sys_rt_exec_state,EXEC_CYCLE_STOP);}}if(sys.state&(STATE_CYCLE|STATE_HOLD|STATE_MOTION_CANCEL)){st_prep_buffer();}}//同步voidprotocol_buffer_synchronize(){protocol_auto_cycle_start();do{protocol_execute_realtime();if(sys.abort){return;}while(plan_get_current_block()||(sys.state==STATE_CYCLE));}voidprotocol_auto_cycle_start(){bit_true_atomic(sys_rt_exec_state,EXEC_CYCLE_START);}//planner.h#ifndefplanner_h#defineplanner_h#ifndefBLOCK_BUFFER_SIZE#ifdefUSE_LINE_NUMBERS#defineBLOCK_BUFFER_SIZE16#else#defineBLOCK_BUFFER_SIZE18#endif#endif//結(jié)構(gòu)定義typedefstruct{uint8_tdirection_bits;uint32_tsteps[N_AXIS];uint32_tstep_event_count;floatentry_speed_sqr;floatmax_entry_speed_sqr;floatmax_junction_speed_sqr;floatnominal_speed_sqr;floatacceleration;floatmillimeters;#ifdefUSE_LINE_NUMBERSint32_tline_number;#endif}plan_block_t;//初始化運(yùn)動(dòng)系統(tǒng)voidplan_reset();#ifdefUSE_LINE_NUMBERSvoidplan_buffer_line(float*target,floatfeed_rate,uint8_tinvert_feed_rate,int32_tline_number);#elsevoidplan_buffer_line(float*target,floatfeed_rate,uint8_tinvert_feed_rate);#endifvoidplan_discard_current_block();plan_block_t*plan_get_current_block();uint8_tplan_next_block_index(uint8_tblock_index);floatplan_get_exec_block_exit_speed();voidplan_sync_position();voidplan_cycle_reinitialize();uint8_tplan_get_block_buffer_count();uint8_tplan_check_full_buffer();#endif#include"library.h"#defineSOME_LARGE_VALUE1.0E+38//靜態(tài)變量staticplan_block_tblock_buffer[BLOCK_BUFFER_SIZE];staticuint8_tblock_buffer_tail;staticuint8_tblock_buffer_head;staticuint8_tnext_buffer_head;staticuint8_tblock_buffer_planned;//定義隊(duì)列變量typedefstruct{int32_tposition[N_AXIS];floatprevious_unit_vec[N_AXIS];floatprevious_nominal_speed_sqr;}planner_t;staticplanner_tpl;uint8_tplan_next_block_index(uint8_tblock_index){block_index++;if(block_index==BLOCK_BUFFER_SIZE){block_index=0;}return(block_index);}staticuint8_tplan_prev_block_index(uint8_tblock_index){if(block_index==0){block_index=BLOCK_BUFFER_SIZE;}block_index--;return(block_index);}staticvoidplanner_recalculate(){uint8_tblock_index=plan_prev_block_index(block_buffer_head);if(block_index==block_buffer_planned){return;}floatentry_speed_sqr;plan_block_t*next;plan_block_t*current=&block_buffer[block_index];current->entry_speed_sqr=min(current->max_entry_speed_sqr,2*current->acceleration*current->millimeters);block_index=plan_prev_block_index(block_index);if(block_index==block_buffer_planned){if(block_index==block_buffer_tail){st_update_plan_block_parameters();}}else{while(block_index!=block_buffer_planned){next=current;current=&block_buffer[block_index];block_index=plan_prev_block_index(block_index);if(block_index==block_buffer_tail){st_update_plan_block_parameters();}if(current->entry_speed_sqr!=current->max_entry_speed_sqr)