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