d5e30201f864ef8a2a79eccb6dfbd25acb6638ce
[fpga/rpi-motor-control.git] / 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 pwm a rizeni. Enable bity na nulu.");
20         puts("0 - Vypne pwm a rizeni. Enable bity na nulu.");
21         puts("ga:[hodnota] - Zapne rizeni na zvolenou absolutni pozici.");
22         puts("print - Zapne nebo vypne pravidelne vypisovani hodnot.");
23         puts("help - Vypne vypisovani hodnot a zobrazi tuto napovedu.");
24         puts("exit - Bezpecne ukonci program.");
25 }
26
27
28 /*
29  * \brief
30  * V prikazech je hodnota od zneni prikazu delena dvojteckou
31  * tato funkce dvojtecku nahradi mezerou
32  */
33 static void delCol(char * txt){
34         unsigned i=0;
35         while(txt[i]!='\0'){
36                 if (txt[i]==':') txt[i]=' ';
37                 i++;
38         }
39 }
40
41 /*
42  * Nastavi enable bity na pwm,
43  * zapne komutaci
44  */
45 static void start(struct rpi_state* state){
46         sem_wait(&state->thd_par_sem);
47         state->test=0x70;       /*konfiguracni byte*/
48         state->commutate=1;
49         sem_post(&state->thd_par_sem);
50 }
51
52 /*
53  * \brief
54  * Zastavi komutaci, vypne pwm
55  */
56 static void stop(struct rpi_state* state){
57         sem_wait(&state->thd_par_sem);
58         state->test=0;          /*konfiguracni byte*/
59         state->commutate=0;
60         state->pwm1=0;
61         state->pwm2=0;
62         state->pwm3=0;
63         sem_post(&state->thd_par_sem);
64 }
65
66 /*
67  * \brief
68  * Nastavi pevnou sirku plneni
69  */
70 static void dutySet(struct rpi_state* state, int duty){
71
72 }
73
74 /*
75  * \brief
76  * Zapne rizeni na zvolenou polohu vztazenou k pozici pri startu
77  */
78 static void goAbsolute(struct rpi_state* state, int pos){
79         sem_wait(&state->thd_par_sem);
80         state->desired_pos=pos;
81         sem_post(&state->thd_par_sem);
82 }
83
84 /*
85  * \brief
86  * Zapne nebo vypne pravidelne vypisovani hodnot.
87  */
88 static void changePrint(){
89         doPrint=!doPrint;
90 }
91
92 /*
93  * \brief
94  * Bezpecne ukonci program.
95  */
96 static void exitApp(struct rpi_state* state){
97         stop(state);
98         /* Note: atexit() is set before*/
99         exit(0);
100 }
101
102 /**
103  * \brief
104  * Commands detection.
105  */
106 void poll_cmd(struct rpi_state* state){
107         unsigned int tmp;
108         char buff[50];
109         char cmd[30];
110         int val;
111
112         while(1){
113                 scanf("%49s",buff);
114                 delCol(buff);
115                 sscanf(buff,"%s %d",cmd,&val);
116
117
118                 if (!strcmp(cmd,"start")){
119                         start(state);
120                 }else if (!strcmp(cmd,"0")){
121                         stop(state);
122                 }else if (!strcmp(cmd,"stop")){
123                         stop(state);
124                 }else if (!strcmp(cmd,"ga")){
125                         goAbsolute(state,val);
126                 }else if (!strcmp(cmd,"duty")){
127                         dutySet(state,val);
128                 }else if (!strcmp(cmd,"help")){
129                         printHelp();
130                 }else if (!strcmp(cmd,"print")){
131                         changePrint();
132                 }else if (!strcmp(cmd,"exit")){
133                         exitApp(state);
134                 }
135         }
136
137 }
138
139 /*
140  * pocita procentualni odchylku od prumerneho proudu
141  */
142 float diff_p(float value){
143         return ((float)value-PRUM_PROUD)*100/PRUM_PROUD;
144 }
145 /*
146  * pocita procentualni odchylku od prumerneho souctu proudu
147  */
148 float diff_s(float value){
149         return ((float)value-PRUM_SOUC)*100/PRUM_SOUC;
150 }
151
152 /*
153  * tiskne potrebna data
154  */
155 void printData(struct rpi_state* state){
156         if (!doPrint) return;
157
158         struct rpi_in data_p;
159         struct rpi_state s;     /*state*/
160         float cur0, cur1, cur2;
161         int i;
162         /* copy the data */
163         sem_wait(&state->thd_par_sem);
164         data_p = *state->spi_dat;
165         s=*state;
166         sem_post(&state->thd_par_sem);
167
168         if (data_p.adc_m_count){
169                 cur0=data_p.ch0/data_p.adc_m_count;
170                 cur1=data_p.ch1/data_p.adc_m_count;
171                 cur2=data_p.ch2/data_p.adc_m_count;
172         }
173         for (i = 0; i < 16; i++) {
174                         if (!(i % 6))
175                                 puts("");
176                         printf("%.2X ", data_p.debug_rx[i]);
177         }
178         puts("");
179         printf("\npozice=%d\n",(int32_t)data_p.pozice);
180         printf("chtena pozice=%d\n",s.desired_pos);
181         printf("transfer count=%u\n",s.tf_count);
182         printf("raw_pozice=%u\n",data_p.pozice_raw);
183         printf("raw_pozice last12=%u\n",(data_p.pozice_raw&0x0FFF));
184         printf("index position=%u\n",data_p.index_position);
185         printf("distance to index=%u\n",s.index_dist);
186         printf("hal1=%d, hal2=%d, hal3=%d\n",data_p.hal1,data_p.hal2,data_p.hal3);
187         printf("en1=%d, en2=%d, en3=%d (Last sent)\n",!!(0x40&s.test),!!(0x20&s.test),!!(0x10&s.test));
188         printf("shdn1=%d, shdn2=%d, shdn3=%d (L.s.)\n",!!(0x08&s.test),!!(0x04&s.test),!!(0x02&s.test));
189         printf("  PWM1=%u   PWM2=%u   PWM3=%u\n",s.pwm1,s.pwm2,s.pwm3);
190         printf("T_PWM1=%u T_PWM2=%u T_PWM3=%u\n",s.t_pwm1,s.t_pwm2, s.t_pwm3);
191         printf("Pocet namerenych proudu=%u\n",data_p.adc_m_count);
192         printf("(pwm1) (ch1)=%d (avg=%4.0f) (%2.2f%%)\n",data_p.ch1,cur1,diff_p(cur1));
193         printf("(pwm2) (ch2)=%d (avg=%4.0f)(%2.2f%%)\n",data_p.ch2,cur2,diff_p(cur2));
194         printf("(pwm3) (ch0)=%d (avg=%4.0f)(%2.2f%%)\n",data_p.ch0,cur0,diff_p(cur0));
195         printf("soucet prumeru=%5.0f (%2.2f%%)\n",cur0+cur1+cur2,diff_s(cur0+cur1+cur2));
196         printf("duty=%d\n",s.duty);
197         if (s.index_ok) printf("index ok\n");
198         if (s.commutate) printf("commutation in progress\n");
199 }