第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)

#includenshared.h"〃共享的定義、變量、函數(shù)

#include"settings.h"http://參數(shù)相關(guān)設(shè)置

#includendefaults.h"〃默認(rèn)設(shè)置

#includencpu_map.h"http://CPU映射

#includeneeprom.hH//EEPROM內(nèi)容存儲(chǔ)

#include“geode.h"http://GCode解讀代碼

#includenmotion_control?h”//運(yùn)動(dòng)控制

#include"planner.h"http://指令隊(duì)列緩存

#include"print.h"http://格式化輸出字符串

#include"probe.hH//系統(tǒng)狀態(tài)探測(cè)

#include"protocol.h"http://控制執(zhí)行協(xié)議

#include"serial.hn//發(fā)送/接收串口通信

#includeHspindle_control.h"http://主軸控制模塊

#include"stepper.h"http://步進(jìn)電機(jī)驅(qū)動(dòng)

#endif

//main.c

#include"library.h"

//系統(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//弓I腳AO

#defineFEED_HOLD_BIT1//引腳Al

#defineCYCLE_START_BIT2//弓|腳A2

#defineCONTROL_INTPCIE1

#defineCONTROL_INT_vectPCINTl_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

#defineWAVEOREGISTERWGM20

#defineWAVE「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(az0,sizeof(a))

#defineclear_vector_float(a)memset(a,0.0,sizeof(float)*N_AXIS)

#defineclear_vector_long(a)memset(az0.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(xzmask){uint8_tsreg=SREG;cli();(x)|=(mask);SREG

=sreg;}

#definebit_false_atomic(x,mask){uint8_tsreg=SREG;cli();(x)&=~(mask);

SREG=sreg;}

A

#definebit_toggle_atomic(xzmask){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_counterzfloat*float_ptr);

//延遲函數(shù)

voiddelay_ms(uintl6_tms);

voiddelay_us(uint32_tus);

floathypot_f(floatx,floaty);

#endif

//shared.c

#include"library.h"

#defineMAX_INT_DIGITS8//最大位數(shù)

uint8_tread_float(char*linezuint8_t*char_counter,float*float_ptr)

{

char*ptr=line+*char_counter;

unsignedcharc;

〃找到第一個(gè)字符用指針指向它

c=*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-=101;

if(c<=9){

ndigit++；

if(ndigit<=MAX_INT_DIGITS){

if(isdecimal){exp―;}

intval=(((intval<<2)+intval)<<1)+c;

}else{

if(!(isdecimal)){exp++;}//丟棄溢出字符

)

}elseif(c==(('.1'01)&Oxff)&&!(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(uintl6_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_DATAOxff

#ifdefENABLE_XONXOFF

#defineRX_BUFFER_FULL96

#defineRX_BUFFER_LOW64

#defineSEND_XOFF1

#defineSEND_XON2

#defineXOFF_SENT3

#defineXON_SENT4

#defineXOFF_CHAR0x13

#defineXON_CHAR0x11

#endif

voidserial_init();

〃寫(xiě)Ibyte數(shù)據(jù)到TXbuffer中

voidserial_write(uint8_tdata);

//讀取buffer中最前方的Ibyte數(shù)據(jù)

uint8_tserial_read();

//清空讀取buffer

voidserial_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.hn

uint8_tserial_rx_buffer[RX_BUFFER_SIZE];

uint8_tserial_rx_buffer_head=0;

volatileuint8_tserial_rx_buffer_tail=0;

uint8tserialtxbuffer[TXBUFFERSIZE];

uint8_tserial_tx_buffer_head=0;

volatileuint8_tserial_tx_buffer_tail=0;

#ifdefENABLE_XONXOFF

volatileuint8_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<57600

uintl6_tUBRR0_value=((F_CPU/(8L*BAUD_RATE))-1)/2;

UCSROA&=-(1?U2X0);

#else

uintl6_tUBRR0_value=((F_CPU/(4L*BAUD_RATE))-1)/2;

UCSROA|=(1?U2X0);

#endif

UBRROH=UBRR0_value?8;

UBRROL=UBRR0_value;

UCSROB|=l?RXEN0;

UCSROB|=1?TXENO;

UCSROB|=1?RXCIEO;

)

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;

serialtxbufferhead=nexthead;

UCSROB|=(1?UDRIEO);

)

ISR(SERIAL_UDRE){

uint8_ttail=serial_tx_buffer_tail;

#ifdefENABLE_XONXOFF

if(flow_ctrl==SEND_XOFF){

UDRO=XOFF_CHAR;

flow_ctrl=XOFF_SENT;

)

elseif(flow_ctrl==SEND_XON){

UDRO=XON_CHAR;

flow_ctrl=XON_SENT;

}else

#endif{

//發(fā)送數(shù)據(jù)

UDRO=serial_tx_buffer[tail];

tail++;

if(tail==TX_BUFFER_SIZE){tail=0;}

serial_tx_buffer_tail=tail;

}

if(tail==serial_tx_buffer_head){

UCSROB&=~(1?UDRIEO);

}

)

//讀取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_XONXOFF

if((serial_get_rx_buffer_count()<RX_BUFFER_LOW)&&flow_ctrl

XOFF_SENT)(

flow_ctrl=SEND_XON;

UCSROB|=(1?UDRIEO);//ForceTX

)

#endif

returndata;

)

//中斷

ISR(SERIAL_RX){

uint8_tdata=UDRO;

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_XONXOFF

if((serial_get_rx_buffer_count()>=RX_BUFFER_FULL)&&flow_ctrl

XON_SENT){

flow_ctrl=SEND_XOFF;

UCSROB|=(1?UDRIEO);

)

#endif

}

)

)

〃重置讀取buffer

voidserial_reset_read_buffer(){

serial_rx_buffer_tail=serial_rx_buffer_head;

#ifdefENABLE_XONXOFF

flow_ctrl=XON_SENT;

#endif

2.2.3指令隊(duì)列模塊相關(guān)代碼

//system.h

#ifndefsystem_h

#definesystem_h

#include"library.h"

//定義系統(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*stepszuint8_tidx);

voidsystem_convert_array_steps_to_mpos(float*position,int32_t*steps)

#ifdefCOREXY

int32_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_UP

CONTROL_PORT&=~(CONTROL_MASK);

#else

CONTROL_PORT|=CONTROL_MASK;

#endif

CONTROL_PCMSK|=CONTROL_MASK;

PCICR|=(1?CONTROL_INT);//修改中斷

)

ISR(CONTROL_INT_vect)

{

uint8_tpin=(CONTROL_PIN&CONTROL_MASK);

#ifndefINVERT_ALL_CONTROL_PINS

pinA=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_statezEXEC_CYCLE_START);

)

}

)

//執(zhí)行啟動(dòng)腳本

voidsystem_execute_startup(char*line)

uint8tn;

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'$1:case*G':case1C*:case1X':

if(line[(char_counter+l)]!=0){return(STATUS_INVALID_STATEMENT);}

switch(line[char_counter]){

case1$'://打印設(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://檢查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;

casefX'://關(guān)閉警報(bào)鎖

if(sys.state==STATE_ALARM){

report_feedback_message(MESSAGE_ALARM_UNLOCK);

sys.state=STATEIDLE;

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;

case11'://打印信息

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;

case1R*://恢復(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]){

casesettings_restore(SETTINGS_RESTORE_DEFAULTS);break;

case'#T: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_varzline))){

report_status_message(STATUS_SETTING_READ_FAIL);

}else{

report_startup_line(helper_varzline);

)

}

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_counterzSvalue))

{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;

#ifdefCOREXY

if(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.stepspermm[BMOTOR];

}else{

pos=steps[idx]/settings.steps_per_mm[idx];

}

#else

pos=steps[idx]/settings.steps_per_mm[idx];

#endif

return(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)

#ifdefCOREXY

int32_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

#includenlibrary.h"

//定義不同對(duì)話類(lèi)型

#defineCOMMENT_NONE0

#defineCOMMENT_TYPE_PARENTHESES1

#defineCOMMENT_TYPE_SEMICOLON2

staticcharline[LINE_BUFFER_SIZE];//執(zhí)彳亍彳亍號(hào)

//執(zhí)行一行

staticvoidprotocol_execute_line(char*line){

protocol_execute_realtime();

if(sys.abort){return;}

#ifdefREPORT_ECHO_LINE_RECEIVED

report_echo_line_received(line);

#endif

if(line[0]==0){

report_status_message(STATUS_OK);

}elseif(line[0]==T$'){

//系統(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==f\rr)){//行結(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<=11){

}elseif(c==*/*){

}elseif(c=='(*){

comment=COMMENT_TYPE_PARENTHESES;

}elseif(c==*;1){

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*){//大小寫(xiě)轉(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_S0FT_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_statezEXEC_RESET));

)

bit_false_atomic(sys_rt_exec_alarmzOxFF);//情況警報(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_MOT