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