X-Git-Url: https://rtime.felk.cvut.cz/gitweb/fpga/rpi-motor-control.git/blobdiff_plain/e25e3c141a062914c72263faec10ffbbad2328e2..6841e483f0b370a6ea2b41c958945d3db9285c90:/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 a0b0592..2c5fdba 100644 --- a/pmsm-control/test_sw/cmd_proc.c +++ b/pmsm-control/test_sw/cmd_proc.c @@ -3,12 +3,44 @@ #include #include "cmd_proc.h" +#include "logs.h" + +/* + * UDP_CAPABLE definuje, zda je pouzeitelna moznost cteni + * referencnich informaci z jineho zarizeni po protokolu + * UDP. Jde zatim jen o testovaci rezim + */ +#define UDP_CAPABLE + +#ifdef UDP_CAPABLE +#include "udp_cli.h" +#include "misc.h" +#include +#endif + #define PRUM_PROUD 2061 #define PRUM_SOUC 6183 static char doPrint = 1; +#ifdef UDP_CAPABLE +static struct remote_pos_st remote_pos={ + .stop=1 +}; +#endif + +/* + * zastavi cteni po UDP + */ +static inline void stop_udp_SEM(){ + #ifdef UDP_CAPABLE + remote_pos.stop=1; + #endif +} + + + /* * \brief * Help @@ -22,6 +54,12 @@ static void printHelp(){ puts("duty:[hodnota] - Nastavi pevnou sirku plneni."); puts("spd:[hodnota] - Zapne rizeni na danou rychlost."); puts("log - Spusti nebo ulozi logovani."); + puts("ao:[hodnota] - Prenastavi alpha offset."); + + #ifdef UDP_CAPABLE + puts("udp_pos:[ip] - Bude ridit pozici na hodnotu vzdaleneho zarizeni"); + puts("udp_spd:[ip] - Bude ridit rychlost na hodnotu vzdaleneho zarizeni"); + #endif puts("print - Zapne nebo vypne pravidelne vypisovani hodnot."); puts("help - Vypne vypisovani hodnot a zobrazi tuto napovedu."); @@ -57,8 +95,11 @@ static void start(struct rpi_state* state){ * Zastavi komutaci, vypne pwm */ static void stop(struct rpi_state* state){ + stop_udp_SEM(); sem_wait(&state->thd_par_sem); - state->commutate=0; + setCommutationOff(state); + setRegulationOff(state); + state->duty=0; state->pwm1=0; state->pwm2=0; state->pwm3=0; @@ -70,13 +111,13 @@ static void stop(struct rpi_state* state){ * Nastavi pevnou sirku plneni */ static void dutySet(struct rpi_state* state, int duty){ + stop_udp_SEM(); sem_wait(&state->thd_par_sem); if (duty>MAX_DUTY) duty=MAX_DUTY; if (duty<-MAX_DUTY) duty=-MAX_DUTY;/*paranoia*/ state->duty=duty; - state->pos_reg_ena=0; - state->spd_reg_ena=0; - state->commutate=1; + setRegulationOff(state); + setCommutationOn(state); sem_post(&state->thd_par_sem); } @@ -85,10 +126,10 @@ static void dutySet(struct rpi_state* state, int duty){ * Zapne rizeni na zvolenou polohu vztazenou k pozici pri startu */ static void goAbsolute(struct rpi_state* state, int pos){ + stop_udp_SEM(); sem_wait(&state->thd_par_sem); - state->spd_reg_ena=0; - state->pos_reg_ena=1; - state->commutate=1; + setRegulationPos(state); + setCommutationOn(state); state->desired_pos=pos; sem_post(&state->thd_par_sem); } @@ -117,53 +158,24 @@ static void exitApp(struct rpi_state* state){ static void setSpeed(struct rpi_state* state, int speed){ if (speed>MAX_SPEED) speed=MAX_SPEED; if (speed<-MAX_SPEED) speed=-MAX_SPEED;/*paranoia*/ + stop_udp_SEM(); sem_wait(&state->thd_par_sem); - state->pos_reg_ena=0; - state->spd_reg_ena=1; - state->commutate=1; + setRegulationSpeed(state); + setCommutationOn(state); state->desired_spd=speed; sem_post(&state->thd_par_sem); } /* * \brief - * Initialize logs + * Set alpha offset. */ -static void logInit(struct rpi_state* state){ - int r; - state->log_col=0; - state->log_col_count=LOG_DEF_COL; - for (r=0;rlogs[r]=malloc(state->log_col_count*sizeof(int)); - } -} - -/* - * \brief - * Save logs - */ -static void saveLogs(struct rpi_state* state){ - FILE *f; - int r,s; - - f = fopen("logs.txt", "w"); - if (f == NULL){ - printf("Error opening file!\n"); - return; - } - - for (r=0;rlog_col-1;s++){ /*posledni sloupec je vevyplneny*/ - if (s==state->log_col-2){ - fprintf(f,"%d ",state->logs[r][s]); - }else{ - fprintf(f,"%d, ",state->logs[r][s]); - } - } - fprintf(f,"\r"); - } - fclose(f); - freeLogs(); +static void setAlphaOff(struct rpi_state* state, int offset){ + if (offset<0) offset*=-1; + offset%=1000; + sem_wait(&state->thd_par_sem); + state->alpha_offset=(unsigned short)offset; + sem_post(&state->thd_par_sem); } /** @@ -182,11 +194,38 @@ static void setLogSEM(struct rpi_state* state){ /* spustim zachytavani logu */ }else{ logInit(state); - state->doLogs=1; sem_post(&state->thd_par_sem); } } +#ifdef UDP_CAPABLE +static void startUdpPos(struct rpi_state* state, char *ip){ + pthread_t thread_id=pthread_self(); + if (!remote_pos.stop) return; /*dont be reentrant*/ + setCommutationOn(state); + setRegulationPos(state); + remote_pos.stop=0; + remote_pos.ip=ip; + remote_pos.factor=-5; + remote_pos.semaphore=&state->thd_par_sem; + remote_pos.rem_pos=&state->desired_pos; + create_rt_task(&thread_id,48,start_reading_remote_position,&remote_pos); +} + +/*FIXME thers no max speed limit check!!! hard debug mode!! be careful*/ +static void startUdpSpd(struct rpi_state* state, char *ip){ + pthread_t thread_id=pthread_self(); + if (!remote_pos.stop) return; /*dont be reentrant*/ + setCommutationOn(state); + setRegulationSpeed(state); + remote_pos.stop=0; + remote_pos.ip=ip; + remote_pos.factor=-1; + remote_pos.semaphore=&state->thd_par_sem; + remote_pos.rem_pos=&state->desired_spd; + create_rt_task(&thread_id,48,start_reading_remote_position,&remote_pos); +} +#endif /** * \brief * Commands detection. @@ -195,12 +234,14 @@ void poll_cmd(struct rpi_state* state){ unsigned int tmp; char buff[50]; char cmd[30]; + char cmd2[30]; int val; while(1){ scanf("%49s",buff); delCol(buff); sscanf(buff,"%s %d",cmd,&val); + sscanf(buff,"%s %s",cmd,cmd2); if (!strcmp(cmd,"start")){ @@ -223,7 +264,18 @@ void poll_cmd(struct rpi_state* state){ setSpeed(state, val); }else if (!strcmp(cmd,"log")){ setLogSEM(state); + }else if (!strcmp(cmd,"ao")){ + setAlphaOff(state,val); } + #ifdef UDP_CAPABLE + else if (!strcmp(cmd,"udp_pos")){ + startUdpPos(state,cmd2); + }else if (!strcmp(cmd,"udp_spd")){ + startUdpSpd(state,cmd2); + + } + #endif + } } @@ -272,6 +324,7 @@ void printData(struct rpi_state* state){ printf("rychlost=%d\n",s.speed); printf("chtena pozice=%d\n",s.desired_pos); printf("chtena rychlost=%d\n",s.desired_spd); + printf("alpha offset=%hu\n",s.alpha_offset); 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)); @@ -290,4 +343,7 @@ void printData(struct rpi_state* state){ printf("duty=%d\n",s.duty); if (s.index_ok) printf("index ok\n"); if (s.commutate) printf("commutation in progress\n"); + if (s.doLogs) printf("logujeme\n"); + if (s.error) printf("error pri maloc logs!! \n"); + }