X-Git-Url: https://rtime.felk.cvut.cz/gitweb/fpga/rpi-motor-control.git/blobdiff_plain/567c062529d0595b8ea1c52e16ad2cc1ed7579b0..1a8d767b097d1536b264d8bae3e9d5c16617f835:/pmsm-control/test_sw/cmd_proc.c diff --git a/pmsm-control/test_sw/cmd_proc.c b/pmsm-control/test_sw/cmd_proc.c index 72260b5..46ff070 100644 --- a/pmsm-control/test_sw/cmd_proc.c +++ b/pmsm-control/test_sw/cmd_proc.c @@ -1,8 +1,112 @@ #include #include +#include #include "cmd_proc.h" +#define PRUM_PROUD 2061 +#define PRUM_SOUC 6183 + +static char doPrint = 1; + +/* + * \brief + * Help + */ +static void printHelp(){ + doPrint=0; + puts("start - Pripravi rizeni, zapne enable bity pwm."); + puts("stop - Vypne pwm a rizeni. Enable bity na nulu."); + puts("0 - Vypne pwm a rizeni. Enable bity na nulu."); + puts("ga:[hodnota] - Zapne rizeni na zvolenou absolutni pozici."); + puts("print - Zapne nebo vypne pravidelne vypisovani hodnot."); + puts("duty:[hodnota] - Nastavi pevnou sirku plneni."); + + puts("help - Vypne vypisovani hodnot a zobrazi tuto napovedu."); + puts("exit - Bezpecne ukonci program."); +} + + +/* + * \brief + * V prikazech je hodnota od zneni prikazu delena dvojteckou + * tato funkce dvojtecku nahradi mezerou + */ +static void delCol(char * txt){ + unsigned i=0; + while(txt[i]!='\0'){ + if (txt[i]==':') txt[i]=' '; + i++; + } +} + +/* + * Nastavi enable bity na pwm, + * zapne komutaci + */ +static void start(struct rpi_state* state){ + sem_wait(&state->thd_par_sem); + state->test=0x70; /*konfiguracni byte*/ + sem_post(&state->thd_par_sem); +} + +/* + * \brief + * Zastavi komutaci, vypne pwm + */ +static void stop(struct rpi_state* state){ + sem_wait(&state->thd_par_sem); + state->test=0; /*konfiguracni byte*/ + state->commutate=0; + state->pos_reg_ena=0; + state->pwm1=0; + state->pwm2=0; + state->pwm3=0; + sem_post(&state->thd_par_sem); +} + +/* + * \brief + * Nastavi pevnou sirku plneni + */ +static void dutySet(struct rpi_state* state, int duty){ + sem_wait(&state->thd_par_sem); + if (duty>512) duty=512; + if (duty<-512) duty=-512;/*paranoia*/ + state->duty=duty; + state->commutate=1; + state->pos_reg_ena=0; + sem_post(&state->thd_par_sem); +} + +/* + * \brief + * Zapne rizeni na zvolenou polohu vztazenou k pozici pri startu + */ +static void goAbsolute(struct rpi_state* state, int pos){ + sem_wait(&state->thd_par_sem); + state->pos_reg_ena=1; + state->desired_pos=pos; + sem_post(&state->thd_par_sem); +} + +/* + * \brief + * Zapne nebo vypne pravidelne vypisovani hodnot. + */ +static void changePrint(){ + doPrint=!doPrint; +} + +/* + * \brief + * Bezpecne ukonci program. + */ +static void exitApp(struct rpi_state* state){ + stop(state); + /* Note: atexit() is set before*/ + exit(0); +} /** * \brief @@ -10,65 +114,95 @@ */ void poll_cmd(struct rpi_state* state){ unsigned int tmp; - /* - * Note: - * pri pouziti scanf("%u",&simple_hall_duty); dochazelo - * k preukladani hodnot na promenne test. Dost divne. - */ - while (1){ - scanf("%u",&tmp); - printf("volba=%u\n",tmp); - switch (tmp){ - case 1: - scanf("%u",&tmp); - sem_wait(&state->thd_par_sem); - state->pwm1=tmp&0xFFF; - sem_post(&state->thd_par_sem); - break; - case 2: - scanf("%u",&tmp); - sem_wait(&state->thd_par_sem); - state->pwm2=tmp&0xFFF; - sem_post(&state->thd_par_sem); - break; - case 3: - scanf("%u",&tmp); - sem_wait(&state->thd_par_sem); - state->pwm3=tmp&0xFFF; - sem_post(&state->thd_par_sem); - break; - case 4: - scanf("%u",&tmp); - sem_wait(&state->thd_par_sem); - state->test=tmp&0xFF; - sem_post(&state->thd_par_sem); - break; - case 5: - sem_wait(&state->thd_par_sem); - state->commutate=!state->commutate; - /* switch off pwms at the end of commutation */ - state->pwm1&=state->commutate*0xFFFF; - state->pwm2&=state->commutate*0xFFFF; - state->pwm3&=state->commutate*0xFFFF; - sem_post(&state->thd_par_sem); - break; - case 6: - scanf("%d",&tmp); - sem_wait(&state->thd_par_sem); - state->duty=tmp; - sem_post(&state->thd_par_sem); - break; - case 7: - scanf("%d",&tmp); - sem_wait(&state->thd_par_sem); - state->desired_pos=tmp; - sem_post(&state->thd_par_sem); - break; - - default: - break; - } + char buff[50]; + char cmd[30]; + int val; + + while(1){ + scanf("%49s",buff); + delCol(buff); + sscanf(buff,"%s %d",cmd,&val); + + if (!strcmp(cmd,"start")){ + start(state); + }else if (!strcmp(cmd,"0")){ + stop(state); + }else if (!strcmp(cmd,"stop")){ + stop(state); + }else if (!strcmp(cmd,"ga")){ + goAbsolute(state,val); + }else if (!strcmp(cmd,"duty")){ + dutySet(state,val); + }else if (!strcmp(cmd,"help")){ + printHelp(); + }else if (!strcmp(cmd,"print")){ + changePrint(); + }else if (!strcmp(cmd,"exit")){ + exitApp(state); + } + } + +} + +/* + * pocita procentualni odchylku od prumerneho proudu + */ +float diff_p(float value){ + return ((float)value-PRUM_PROUD)*100/PRUM_PROUD; +} +/* + * pocita procentualni odchylku od prumerneho souctu proudu + */ +float diff_s(float value){ + return ((float)value-PRUM_SOUC)*100/PRUM_SOUC; +} + +/* + * tiskne potrebna data + */ +void printData(struct rpi_state* state){ + if (!doPrint) return; + + struct rpi_in data_p; + struct rpi_state s; /*state*/ + float cur0, cur1, cur2; + int i; + /* copy the data */ + sem_wait(&state->thd_par_sem); + data_p = *state->spi_dat; + s=*state; + sem_post(&state->thd_par_sem); + + if (data_p.adc_m_count){ + cur0=data_p.ch0/data_p.adc_m_count; + cur1=data_p.ch1/data_p.adc_m_count; + cur2=data_p.ch2/data_p.adc_m_count; + } + for (i = 0; i < 16; i++) { + if (!(i % 6)) + puts(""); + printf("%.2X ", data_p.debug_rx[i]); } - return ; + puts(""); + printf("\npozice=%d\n",(int32_t)data_p.pozice); + printf("chtena pozice=%d\n",s.desired_pos); + printf("transfer count=%u\n",s.tf_count); + printf("raw_pozice=%u\n",data_p.pozice_raw); + printf("raw_pozice last12=%u\n",(data_p.pozice_raw&0x0FFF)); + printf("index position=%u\n",data_p.index_position); + printf("distance to index=%u\n",s.index_dist); + printf("hal1=%d, hal2=%d, hal3=%d\n",data_p.hal1,data_p.hal2,data_p.hal3); + printf("en1=%d, en2=%d, en3=%d (Last sent)\n",!!(0x40&s.test),!!(0x20&s.test),!!(0x10&s.test)); + printf("shdn1=%d, shdn2=%d, shdn3=%d (L.s.)\n",!!(0x08&s.test),!!(0x04&s.test),!!(0x02&s.test)); + printf(" PWM1=%u PWM2=%u PWM3=%u\n",s.pwm1,s.pwm2,s.pwm3); + printf("T_PWM1=%u T_PWM2=%u T_PWM3=%u\n",s.t_pwm1,s.t_pwm2, s.t_pwm3); + printf("Pocet namerenych proudu=%u\n",data_p.adc_m_count); + printf("(pwm1) (ch1)=%d (avg=%4.0f) (%2.2f%%)\n",data_p.ch1,cur1,diff_p(cur1)); + printf("(pwm2) (ch2)=%d (avg=%4.0f)(%2.2f%%)\n",data_p.ch2,cur2,diff_p(cur2)); + printf("(pwm3) (ch0)=%d (avg=%4.0f)(%2.2f%%)\n",data_p.ch0,cur0,diff_p(cur0)); + printf("soucet prumeru=%5.0f (%2.2f%%)\n",cur0+cur1+cur2,diff_s(cur0+cur1+cur2)); + printf("duty=%d\n",s.duty); + if (s.index_ok) printf("index ok\n"); + if (s.commutate) printf("commutation in progress\n"); }