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