pmsm-control/test_sw/cmd_proc.c

   1 #include <stdlib.h>
   2 #include <stdio.h>
   3 #include <string.h>
   4
   5 #include "cmd_proc.h"
   6 #include "logs.h"
   7
   8 /*
   9  * UDP_CAPABLE definuje, zda je pouzeitelna moznost cteni
  10  * referencnich informaci z jineho zarizeni po protokolu
  11  * UDP. Jde zatim jen o testovaci rezim
  12  */
  13 #define UDP_CAPABLE_X
  14
  15 #ifdef UDP_CAPABLE
  16 #include "udp_cli.h"
  17 #include "misc.h"
  18 #include <pthread.h>
  19 #endif
  20
  21
  22 #define PRUM_PROUD      2061
  23 #define PRUM_SOUC       6183
  24
  25 static char doPrint = 1;
  26
  27 #ifdef UDP_CAPABLE
  28 static struct remote_pos_st remote_pos={
  29                 .stop=1
  30 };
  31 #endif
  32
  33 /*
  34  * zastavi cteni po UDP
  35  */
  36 static inline void stop_udp_SEM(){
  37         #ifdef UDP_CAPABLE
  38         remote_pos.stop=1;
  39         #endif
  40 }
  41
  42
  43
  44 /*
  45  * \brief
  46  * Help
  47  */
  48 static void printHelp(){
  49         doPrint=0;
  50         puts("start - Pripravi rizeni, zapne enable bity pwm.");
  51         puts("stop - Vypne komutaci, pwm a rizeni.");
  52         puts("0 - Vypne komutaci, pwm a rizeni.");
  53         puts("ga:[hodnota] - Zapne rizeni na zvolenou absolutni pozici.");
  54         puts("duty:[hodnota] - Nastavi pevnou sirku plneni.");
  55         puts("spd:[hodnota] - Zapne rizeni na danou rychlost.");
  56         puts("log - Spusti nebo ulozi logovani.");
  57         puts("ao:[hodnota] - Prenastavi alpha offset.");
  58
  59         #ifdef UDP_CAPABLE
  60         puts("udp_pos:[ip] - Bude ridit pozici na hodnotu vzdaleneho zarizeni");
  61         puts("udp_spd:[ip] - Bude ridit rychlost na hodnotu vzdaleneho zarizeni");
  62         #endif
  63
  64         puts("print - Zapne nebo vypne pravidelne vypisovani hodnot.");
  65         puts("help - Vypne vypisovani hodnot a zobrazi tuto napovedu.");
  66         puts("exit - Bezpecne ukonci program.");
  67 }
  68
  69
  70 /*
  71  * \brief
  72  * V prikazech je hodnota od zneni prikazu delena dvojteckou
  73  * tato funkce dvojtecku nahradi mezerou
  74  */
  75 static void delCol(char * txt){
  76         unsigned i=0;
  77         while(txt[i]!='\0'){
  78                 if (txt[i]==':') txt[i]=' ';
  79                 i++;
  80         }
  81 }
  82
  83 /*
  84  * Nastavi enable bity na pwm,
  85  * zapne komutaci
  86  */
  87 static void start(struct rpi_state* state){
  88         sem_wait(&state->thd_par_sem);
  89         state->test=0x70;       /*konfiguracni byte*/
  90         sem_post(&state->thd_par_sem);
  91 }
  92
  93 /*
  94  * \brief
  95  * Zastavi komutaci, vypne pwm
  96  */
  97 static void stop(struct rpi_state* state){
  98         stop_udp_SEM();
  99         sem_wait(&state->thd_par_sem);
 100         setCommutationOff(state);
 101         setRegulationOff(state);
 102         state->duty=0;
 103         state->pwm1=0;
 104         state->pwm2=0;
 105         state->pwm3=0;
 106         sem_post(&state->thd_par_sem);
 107 }
 108
 109 /*
 110  * \brief
 111  * Nastavi pevnou sirku plneni
 112  */
 113 static void dutySet(struct rpi_state* state, int duty){
 114         stop_udp_SEM();
 115         sem_wait(&state->thd_par_sem);
 116         if (duty>MAX_DUTY) duty=MAX_DUTY;
 117         if (duty<-MAX_DUTY) duty=-MAX_DUTY;/*paranoia*/
 118         state->duty=duty;
 119         setRegulationOff(state);
 120         setCommutationOn(state);
 121         sem_post(&state->thd_par_sem);
 122 }
 123
 124 /*
 125  * \brief
 126  * Zapne rizeni na zvolenou polohu vztazenou k pozici pri startu
 127  */
 128 static void goAbsolute(struct rpi_state* state, int pos){
 129         stop_udp_SEM();
 130         sem_wait(&state->thd_par_sem);
 131         setRegulationPos(state);
 132         setCommutationOn(state);
 133         state->desired_pos=pos;
 134         sem_post(&state->thd_par_sem);
 135 }
 136
 137 /*
 138  * \brief
 139  * Zapne nebo vypne pravidelne vypisovani hodnot.
 140  */
 141 static void changePrint(){
 142         doPrint=!doPrint;
 143 }
 144
 145 /*
 146  * \brief
 147  * Bezpecne ukonci program.
 148  */
 149 static void exitApp(struct rpi_state* state){
 150         stop(state);
 151         /* Note: atexit() is set before*/
 152         exit(0);
 153 }
 154 /*
 155  * \brief
 156  * Set speed.
 157  */
 158 static void setSpeed(struct rpi_state* state, int speed){
 159         if (speed>MAX_SPEED) speed=MAX_SPEED;
 160         if (speed<-MAX_SPEED) speed=-MAX_SPEED;/*paranoia*/
 161         stop_udp_SEM();
 162         sem_wait(&state->thd_par_sem);
 163         setRegulationSpeed(state);
 164         setCommutationOn(state);
 165         state->desired_spd=speed;
 166         sem_post(&state->thd_par_sem);
 167 }
 168
 169 /*
 170  * \brief
 171  * Set alpha offset.
 172  */
 173 static void setAlphaOff(struct rpi_state* state, int offset){
 174         if (offset<0) offset*=-1;
 175         offset%=1000;
 176         sem_wait(&state->thd_par_sem);
 177         state->alpha_offset=(unsigned short)offset;
 178         sem_post(&state->thd_par_sem);
 179 }
 180
 181 /**
 182  * \brief
 183  * SetLog
 184  * if logs are being logged, save them
 185  * if they are not, start log them
 186  */
 187 static void setLogSEM(struct rpi_state* state){
 188         sem_wait(&state->thd_par_sem);
 189         /* ulozim logy a vypnu zachytavani*/
 190         if (state->log_col_count){
 191                 state->doLogs=0;
 192                 sem_post(&state->thd_par_sem);
 193                 saveLogs(state);
 194         /* spustim zachytavani logu */
 195         }else{
 196                 logInit(state);
 197                 sem_post(&state->thd_par_sem);
 198         }
 199 }
 200
 201 #ifdef UDP_CAPABLE
 202 static void  startUdpPos(struct rpi_state* state, char *ip){
 203         pthread_t thread_id=pthread_self();
 204         if (!remote_pos.stop) return; /*dont be reentrant*/
 205         setCommutationOn(state);
 206         setRegulationPos(state);
 207         remote_pos.stop=0;
 208         remote_pos.ip=ip;
 209         remote_pos.factor=-5;
 210         remote_pos.semaphore=&state->thd_par_sem;
 211         remote_pos.rem_pos=&state->desired_pos;
 212         create_rt_task(&thread_id,48,start_reading_remote_position,&remote_pos);
 213 }
 214
 215 /*FIXME thers no max speed limit check!!! hard debug mode!! be careful*/
 216 static void  startUdpSpd(struct rpi_state* state, char *ip){
 217         pthread_t thread_id=pthread_self();
 218         if (!remote_pos.stop) return; /*dont be reentrant*/
 219         setCommutationOn(state);
 220         setRegulationSpeed(state);
 221         remote_pos.stop=0;
 222         remote_pos.ip=ip;
 223         remote_pos.factor=-1;
 224         remote_pos.semaphore=&state->thd_par_sem;
 225         remote_pos.rem_pos=&state->desired_spd;
 226         create_rt_task(&thread_id,48,start_reading_remote_position,&remote_pos);
 227 }
 228 #endif
 229 /**
 230  * \brief
 231  * Commands detection.
 232  */
 233 void poll_cmd(struct rpi_state* state){
 234         unsigned int tmp;
 235         char buff[50];
 236         char cmd[30];
 237         char cmd2[30];
 238         int val;
 239
 240         while(1){
 241                 scanf("%49s",buff);
 242                 delCol(buff);
 243                 sscanf(buff,"%s %d",cmd,&val);
 244                 sscanf(buff,"%s %s",cmd,cmd2);
 245
 246
 247                 if (!strcmp(cmd,"start")){
 248                         start(state);
 249                 }else if (!strcmp(cmd,"0")){
 250                         stop(state);
 251                 }else if (!strcmp(cmd,"stop")){
 252                         stop(state);
 253                 }else if (!strcmp(cmd,"ga")){
 254                         goAbsolute(state,val);
 255                 }else if (!strcmp(cmd,"duty")){
 256                         dutySet(state,val);
 257                 }else if (!strcmp(cmd,"help")){
 258                         printHelp();
 259                 }else if (!strcmp(cmd,"print")){
 260                         changePrint();
 261                 }else if (!strcmp(cmd,"exit")){
 262                         exitApp(state);
 263                 }else if (!strcmp(cmd,"spd")){
 264                         setSpeed(state, val);
 265                 }else if (!strcmp(cmd,"log")){
 266                         setLogSEM(state);
 267                 }else if (!strcmp(cmd,"ao")){
 268                         setAlphaOff(state,val);
 269                 }
 270                 #ifdef UDP_CAPABLE
 271                 else if (!strcmp(cmd,"udp_pos")){
 272                         startUdpPos(state,cmd2);
 273                 }else if (!strcmp(cmd,"udp_spd")){
 274                         startUdpSpd(state,cmd2);
 275
 276                 }
 277                 #endif
 278
 279         }
 280
 281 }
 282
 283 /*
 284  * pocita procentualni odchylku od prumerneho proudu
 285  */
 286 static float diff_p(float value){
 287         return ((float)value-PRUM_PROUD)*100/PRUM_PROUD;
 288 }
 289 /*
 290  * pocita procentualni odchylku od prumerneho souctu proudu
 291  */
 292 static float diff_s(float value){
 293         return ((float)value-PRUM_SOUC)*100/PRUM_SOUC;
 294 }
 295
 296 /*
 297  * tiskne potrebna data
 298  */
 299 void printData(struct rpi_state* state){
 300         if (!doPrint) return;
 301
 302         struct rpi_in data_p;
 303         struct rpi_state s;     /*state*/
 304         float cur0, cur1, cur2;
 305         int i;
 306         /* copy the data */
 307         sem_wait(&state->thd_par_sem);
 308         data_p = *state->spi_dat;
 309         s=*state;
 310         sem_post(&state->thd_par_sem);
 311
 312         if (data_p.adc_m_count){
 313                 cur0=data_p.ch0/data_p.adc_m_count;
 314                 cur1=data_p.ch1/data_p.adc_m_count;
 315                 cur2=data_p.ch2/data_p.adc_m_count;
 316         }
 317         for (i = 0; i < 16; i++) {
 318                         if (!(i % 6))
 319                                 puts("");
 320                         printf("%.2X ", data_p.debug_rx[i]);
 321         }
 322         puts("");
 323         printf("\npozice=%ld\n",data_p.pozice);
 324         printf("rychlost=%d\n",s.speed);
 325         printf("chtena pozice=%d\n",s.desired_pos);
 326         printf("chtena rychlost=%d\n",s.desired_spd);
 327         printf("alpha offset=%hu\n",s.alpha_offset);
 328         printf("transfer count=%u\n",s.tf_count);
 329         printf("raw_pozice=%u\n",data_p.pozice_raw);
 330         printf("raw_pozice last12=%u\n",(data_p.pozice_raw&0x0FFF));
 331         printf("index position=%u\n",data_p.index_position);
 332         printf("distance to index=%u\n",s.index_dist);
 333         printf("hal1=%d, hal2=%d, hal3=%d\n",data_p.hal1,data_p.hal2,data_p.hal3);
 334         printf("en1=%d, en2=%d, en3=%d (Last sent)\n",!!(0x40&s.test),!!(0x20&s.test),!!(0x10&s.test));
 335         printf("shdn1=%d, shdn2=%d, shdn3=%d (L.s.)\n",!!(0x08&s.test),!!(0x04&s.test),!!(0x02&s.test));
 336         printf("  PWM1=%u   PWM2=%u   PWM3=%u\n",s.pwm1,s.pwm2,s.pwm3);
 337         printf("T_PWM1=%u T_PWM2=%u T_PWM3=%u\n",s.t_pwm1,s.t_pwm2, s.t_pwm3);
 338         printf("Pocet namerenych proudu=%u\n",data_p.adc_m_count);
 339         printf("(pwm1) (ch1)=%d (avg=%4.0f) (%2.2f%%)\n",data_p.ch1,cur1,diff_p(cur1));
 340         printf("(pwm2) (ch2)=%d (avg=%4.0f)(%2.2f%%)\n",data_p.ch2,cur2,diff_p(cur2));
 341         printf("(pwm3) (ch0)=%d (avg=%4.0f)(%2.2f%%)\n",data_p.ch0,cur0,diff_p(cur0));
 342         printf("soucet prumeru=%5.0f (%2.2f%%)\n",cur0+cur1+cur2,diff_s(cur0+cur1+cur2));
 343         printf("duty=%d\n",s.duty);
 344         if (s.index_ok) printf("index ok\n");
 345         if (s.commutate) printf("commutation in progress\n");
 346         if (s.doLogs) printf("logujeme\n");
 347         if (s.error) printf("error pri maloc logs!! \n");
 348
 349 }