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
   7 #define PRUM_PROUD      2061
   8 #define PRUM_SOUC       6183
   9
  10 static char doPrint = 1;
  11 static char error = 0;
  12
  13 /*
  14  * \brief
  15  * Help
  16  */
  17 static void printHelp(){
  18         doPrint=0;
  19         puts("start - Pripravi rizeni, zapne enable bity pwm.");
  20         puts("stop - Vypne komutaci, pwm a rizeni.");
  21         puts("0 - Vypne komutaci, pwm a rizeni.");
  22         puts("ga:[hodnota] - Zapne rizeni na zvolenou absolutni pozici.");
  23         puts("duty:[hodnota] - Nastavi pevnou sirku plneni.");
  24         puts("spd:[hodnota] - Zapne rizeni na danou rychlost.");
  25         puts("log - Spusti nebo ulozi logovani.");
  26
  27         puts("print - Zapne nebo vypne pravidelne vypisovani hodnot.");
  28         puts("help - Vypne vypisovani hodnot a zobrazi tuto napovedu.");
  29         puts("exit - Bezpecne ukonci program.");
  30 }
  31
  32
  33 /*
  34  * \brief
  35  * V prikazech je hodnota od zneni prikazu delena dvojteckou
  36  * tato funkce dvojtecku nahradi mezerou
  37  */
  38 static void delCol(char * txt){
  39         unsigned i=0;
  40         while(txt[i]!='\0'){
  41                 if (txt[i]==':') txt[i]=' ';
  42                 i++;
  43         }
  44 }
  45
  46 /*
  47  * Nastavi enable bity na pwm,
  48  * zapne komutaci
  49  */
  50 static void start(struct rpi_state* state){
  51         sem_wait(&state->thd_par_sem);
  52         state->test=0x70;       /*konfiguracni byte*/
  53         sem_post(&state->thd_par_sem);
  54 }
  55
  56 /*
  57  * \brief
  58  * Zastavi komutaci, vypne pwm
  59  */
  60 static void stop(struct rpi_state* state){
  61         sem_wait(&state->thd_par_sem);
  62         state->commutate=0;
  63         state->pos_reg_ena=0;
  64         state->spd_reg_ena=0;
  65         state->duty=0;
  66         state->pwm1=0;
  67         state->pwm2=0;
  68         state->pwm3=0;
  69         sem_post(&state->thd_par_sem);
  70 }
  71
  72 /*
  73  * \brief
  74  * Nastavi pevnou sirku plneni
  75  */
  76 static void dutySet(struct rpi_state* state, int duty){
  77         sem_wait(&state->thd_par_sem);
  78         if (duty>MAX_DUTY) duty=MAX_DUTY;
  79         if (duty<-MAX_DUTY) duty=-MAX_DUTY;/*paranoia*/
  80         state->duty=duty;
  81         state->pos_reg_ena=0;
  82         state->spd_reg_ena=0;
  83         state->commutate=1;
  84         sem_post(&state->thd_par_sem);
  85 }
  86
  87 /*
  88  * \brief
  89  * Zapne rizeni na zvolenou polohu vztazenou k pozici pri startu
  90  */
  91 static void goAbsolute(struct rpi_state* state, int pos){
  92         sem_wait(&state->thd_par_sem);
  93         state->spd_reg_ena=0;
  94         state->pos_reg_ena=1;
  95         state->commutate=1;
  96         state->desired_pos=pos;
  97         sem_post(&state->thd_par_sem);
  98 }
  99
 100 /*
 101  * \brief
 102  * Zapne nebo vypne pravidelne vypisovani hodnot.
 103  */
 104 static void changePrint(){
 105         doPrint=!doPrint;
 106 }
 107
 108 /*
 109  * \brief
 110  * Bezpecne ukonci program.
 111  */
 112 static void exitApp(struct rpi_state* state){
 113         stop(state);
 114         /* Note: atexit() is set before*/
 115         exit(0);
 116 }
 117 /*
 118  * \brief
 119  * Set speed.
 120  */
 121 static void setSpeed(struct rpi_state* state, int speed){
 122         if (speed>MAX_SPEED) speed=MAX_SPEED;
 123         if (speed<-MAX_SPEED) speed=-MAX_SPEED;/*paranoia*/
 124         sem_wait(&state->thd_par_sem);
 125         state->pos_reg_ena=0;
 126         state->spd_reg_ena=1;
 127         state->commutate=1;
 128         state->desired_spd=speed;
 129         sem_post(&state->thd_par_sem);
 130 }
 131
 132 /*
 133  * \brief
 134  * Initialize logs
 135  */
 136 static void logInit(struct rpi_state* state){
 137         int r;
 138         state->log_col=0;
 139         state->log_col_count=LOG_DEF_COL;
 140         for (r=0;r<LOG_ROWS;r++){
 141                 state->logs[r]=malloc(state->log_col_count*sizeof(int));
 142                 if (state->logs[r]==NULL){
 143                         error=1;
 144                         state->log_col_count=-1;
 145                         return;
 146                 }
 147         }
 148         state->doLogs=1;
 149 }
 150
 151 /*
 152  * \brief
 153  * Save logs
 154  */
 155 static void saveLogs(struct rpi_state* state){
 156         FILE *f;
 157         int r,s;
 158
 159         f = fopen("logs.log", "w");
 160         if (f == NULL){
 161                 printf("Error opening file!\n");
 162                 return;
 163         }
 164
 165         for (r=0;r<LOG_ROWS;r++){
 166                 for(s=0;s<state->log_col-1;s++){        /*posledni sloupec je vevyplneny*/
 167                         if (s==state->log_col-2){
 168                                 fprintf(f,"%d ",state->logs[r][s]);
 169                         }else{
 170                                 fprintf(f,"%d, ",state->logs[r][s]);
 171                         }
 172                 }
 173                 fprintf(f,"\r");
 174         }
 175         fclose(f);
 176         freeLogs();
 177 }
 178
 179 /**
 180  * \brief
 181  * SetLog
 182  * if logs are being logged, save them
 183  * if they are not, start log them
 184  */
 185 static void setLogSEM(struct rpi_state* state){
 186         sem_wait(&state->thd_par_sem);
 187         /* ulozim logy a vypnu zachytavani*/
 188         if (state->log_col_count){
 189                 state->doLogs=0;
 190                 sem_post(&state->thd_par_sem);
 191                 saveLogs(state);
 192         /* spustim zachytavani logu */
 193         }else{
 194                 logInit(state);
 195                 sem_post(&state->thd_par_sem);
 196         }
 197 }
 198
 199 /**
 200  * \brief
 201  * Commands detection.
 202  */
 203 void poll_cmd(struct rpi_state* state){
 204         unsigned int tmp;
 205         char buff[50];
 206         char cmd[30];
 207         int val;
 208
 209         while(1){
 210                 scanf("%49s",buff);
 211                 delCol(buff);
 212                 sscanf(buff,"%s %d",cmd,&val);
 213
 214
 215                 if (!strcmp(cmd,"start")){
 216                         start(state);
 217                 }else if (!strcmp(cmd,"0")){
 218                         stop(state);
 219                 }else if (!strcmp(cmd,"stop")){
 220                         stop(state);
 221                 }else if (!strcmp(cmd,"ga")){
 222                         goAbsolute(state,val);
 223                 }else if (!strcmp(cmd,"duty")){
 224                         dutySet(state,val);
 225                 }else if (!strcmp(cmd,"help")){
 226                         printHelp();
 227                 }else if (!strcmp(cmd,"print")){
 228                         changePrint();
 229                 }else if (!strcmp(cmd,"exit")){
 230                         exitApp(state);
 231                 }else if (!strcmp(cmd,"spd")){
 232                         setSpeed(state, val);
 233                 }else if (!strcmp(cmd,"log")){
 234                         setLogSEM(state);
 235                 }
 236         }
 237
 238 }
 239
 240 /*
 241  * pocita procentualni odchylku od prumerneho proudu
 242  */
 243 static float diff_p(float value){
 244         return ((float)value-PRUM_PROUD)*100/PRUM_PROUD;
 245 }
 246 /*
 247  * pocita procentualni odchylku od prumerneho souctu proudu
 248  */
 249 static float diff_s(float value){
 250         return ((float)value-PRUM_SOUC)*100/PRUM_SOUC;
 251 }
 252
 253 /*
 254  * tiskne potrebna data
 255  */
 256 void printData(struct rpi_state* state){
 257         if (!doPrint) return;
 258
 259         struct rpi_in data_p;
 260         struct rpi_state s;     /*state*/
 261         float cur0, cur1, cur2;
 262         int i;
 263         /* copy the data */
 264         sem_wait(&state->thd_par_sem);
 265         data_p = *state->spi_dat;
 266         s=*state;
 267         sem_post(&state->thd_par_sem);
 268
 269         if (data_p.adc_m_count){
 270                 cur0=data_p.ch0/data_p.adc_m_count;
 271                 cur1=data_p.ch1/data_p.adc_m_count;
 272                 cur2=data_p.ch2/data_p.adc_m_count;
 273         }
 274         for (i = 0; i < 16; i++) {
 275                         if (!(i % 6))
 276                                 puts("");
 277                         printf("%.2X ", data_p.debug_rx[i]);
 278         }
 279         puts("");
 280         printf("\npozice=%ld\n",data_p.pozice);
 281         printf("rychlost=%d\n",s.speed);
 282         printf("chtena pozice=%d\n",s.desired_pos);
 283         printf("chtena rychlost=%d\n",s.desired_spd);
 284         printf("transfer count=%u\n",s.tf_count);
 285         printf("raw_pozice=%u\n",data_p.pozice_raw);
 286         printf("raw_pozice last12=%u\n",(data_p.pozice_raw&0x0FFF));
 287         printf("index position=%u\n",data_p.index_position);
 288         printf("distance to index=%u\n",s.index_dist);
 289         printf("hal1=%d, hal2=%d, hal3=%d\n",data_p.hal1,data_p.hal2,data_p.hal3);
 290         printf("en1=%d, en2=%d, en3=%d (Last sent)\n",!!(0x40&s.test),!!(0x20&s.test),!!(0x10&s.test));
 291         printf("shdn1=%d, shdn2=%d, shdn3=%d (L.s.)\n",!!(0x08&s.test),!!(0x04&s.test),!!(0x02&s.test));
 292         printf("  PWM1=%u   PWM2=%u   PWM3=%u\n",s.pwm1,s.pwm2,s.pwm3);
 293         printf("T_PWM1=%u T_PWM2=%u T_PWM3=%u\n",s.t_pwm1,s.t_pwm2, s.t_pwm3);
 294         printf("Pocet namerenych proudu=%u\n",data_p.adc_m_count);
 295         printf("(pwm1) (ch1)=%d (avg=%4.0f) (%2.2f%%)\n",data_p.ch1,cur1,diff_p(cur1));
 296         printf("(pwm2) (ch2)=%d (avg=%4.0f)(%2.2f%%)\n",data_p.ch2,cur2,diff_p(cur2));
 297         printf("(pwm3) (ch0)=%d (avg=%4.0f)(%2.2f%%)\n",data_p.ch0,cur0,diff_p(cur0));
 298         printf("soucet prumeru=%5.0f (%2.2f%%)\n",cur0+cur1+cur2,diff_s(cur0+cur1+cur2));
 299         printf("duty=%d\n",s.duty);
 300         if (s.index_ok) printf("index ok\n");
 301         if (s.commutate) printf("commutation in progress\n");
 302         if (s.doLogs) printf("logujeme\n");
 303         if (error) printf("error pri maloc logs!! \n");
 304
 305 }