9 * UDP_CAPABLE definuje, zda je pouzeitelna moznost cteni
10 * referencnich informaci z jineho zarizeni po protokolu
11 * UDP. Jde zatim jen o testovaci rezim
22 #define PRUM_PROUD 2061
23 #define PRUM_SOUC 6183
25 static char doPrint = 1;
28 static struct remote_pos_st remote_pos={
34 * zastavi cteni po UDP
36 static inline void stop_udp_SEM(){
48 static void printHelp(){
50 puts("start - Pripravi rizeni, zapne enable bity pwm.");
51 puts("stop - Vypne komutaci, pwm a rizeni.");
52 puts("0 - Vypne komutaci, pwm a rizeni.");
53 puts("ga:[hodnota] - Zapne rizeni na zvolenou absolutni pozici.");
54 puts("duty:[hodnota] - Nastavi pevnou sirku plneni.");
55 puts("spd:[hodnota] - Zapne rizeni na danou rychlost.");
56 puts("log - Spusti nebo ulozi logovani.");
57 puts("ao:[hodnota] - Prenastavi alpha offset.");
60 puts("udp_pos:[ip] - Bude ridit pozici na hodnotu vzdaleneho zarizeni");
61 puts("udp_spd:[ip] - Bude ridit rychlost na hodnotu vzdaleneho zarizeni");
64 puts("print - Zapne nebo vypne pravidelne vypisovani hodnot.");
65 puts("help - Vypne vypisovani hodnot a zobrazi tuto napovedu.");
66 puts("exit - Bezpecne ukonci program.");
72 * V prikazech je hodnota od zneni prikazu delena dvojteckou
73 * tato funkce dvojtecku nahradi mezerou
75 static void delCol(char * txt){
78 if (txt[i]==':') txt[i]=' ';
84 * Nastavi enable bity na pwm,
87 static void start(struct rpi_state* state){
88 sem_wait(&state->thd_par_sem);
89 state->test=0x70; /*konfiguracni byte*/
90 sem_post(&state->thd_par_sem);
95 * Zastavi komutaci, vypne pwm
97 static void stop(struct rpi_state* state){
99 sem_wait(&state->thd_par_sem);
100 setCommutationOff(state);
101 setRegulationOff(state);
106 sem_post(&state->thd_par_sem);
111 * Nastavi pevnou sirku plneni
113 static void dutySet(struct rpi_state* state, int duty){
115 sem_wait(&state->thd_par_sem);
116 if (duty>MAX_DUTY) duty=MAX_DUTY;
117 if (duty<-MAX_DUTY) duty=-MAX_DUTY;/*paranoia*/
119 setRegulationOff(state);
120 setCommutationOn(state);
121 sem_post(&state->thd_par_sem);
126 * Zapne rizeni na zvolenou polohu vztazenou k pozici pri startu
128 static void goAbsolute(struct rpi_state* state, int pos){
130 sem_wait(&state->thd_par_sem);
131 setRegulationPos(state);
132 setCommutationOn(state);
133 state->desired_pos=pos;
134 sem_post(&state->thd_par_sem);
139 * Zapne nebo vypne pravidelne vypisovani hodnot.
141 static void changePrint(){
147 * Bezpecne ukonci program.
149 static void exitApp(struct rpi_state* state){
151 /* Note: atexit() is set before*/
158 static void setSpeed(struct rpi_state* state, int speed){
159 if (speed>MAX_SPEED) speed=MAX_SPEED;
160 if (speed<-MAX_SPEED) speed=-MAX_SPEED;/*paranoia*/
162 sem_wait(&state->thd_par_sem);
163 setRegulationSpeed(state);
164 setCommutationOn(state);
165 state->desired_spd=speed;
166 sem_post(&state->thd_par_sem);
173 static void setAlphaOff(struct rpi_state* state, int offset){
174 if (offset<0) offset*=-1;
176 sem_wait(&state->thd_par_sem);
177 state->alpha_offset=(unsigned short)offset;
178 sem_post(&state->thd_par_sem);
184 * if logs are being logged, save them
185 * if they are not, start log them
187 static void setLogSEM(struct rpi_state* state){
188 sem_wait(&state->thd_par_sem);
189 /* ulozim logy a vypnu zachytavani*/
190 if (state->log_col_count){
192 sem_post(&state->thd_par_sem);
194 /* spustim zachytavani logu */
197 sem_post(&state->thd_par_sem);
202 static void startUdpPos(struct rpi_state* state, char *ip){
203 pthread_t thread_id=pthread_self();
204 if (!remote_pos.stop) return; /*dont be reentrant*/
205 setCommutationOn(state);
206 setRegulationPos(state);
209 remote_pos.factor=-5;
210 remote_pos.semaphore=&state->thd_par_sem;
211 remote_pos.rem_pos=&state->desired_pos;
212 create_rt_task(&thread_id,48,start_reading_remote_position,&remote_pos);
215 /*FIXME thers no max speed limit check!!! hard debug mode!! be careful*/
216 static void startUdpSpd(struct rpi_state* state, char *ip){
217 pthread_t thread_id=pthread_self();
218 if (!remote_pos.stop) return; /*dont be reentrant*/
219 setCommutationOn(state);
220 setRegulationSpeed(state);
223 remote_pos.factor=-1;
224 remote_pos.semaphore=&state->thd_par_sem;
225 remote_pos.rem_pos=&state->desired_spd;
226 create_rt_task(&thread_id,48,start_reading_remote_position,&remote_pos);
231 * Commands detection.
233 void poll_cmd(struct rpi_state* state){
243 sscanf(buff,"%s %d",cmd,&val);
244 sscanf(buff,"%s %s",cmd,cmd2);
247 if (!strcmp(cmd,"start")){
249 }else if (!strcmp(cmd,"0")){
251 }else if (!strcmp(cmd,"stop")){
253 }else if (!strcmp(cmd,"ga")){
254 goAbsolute(state,val);
255 }else if (!strcmp(cmd,"duty")){
257 }else if (!strcmp(cmd,"help")){
259 }else if (!strcmp(cmd,"print")){
261 }else if (!strcmp(cmd,"exit")){
263 }else if (!strcmp(cmd,"spd")){
264 setSpeed(state, val);
265 }else if (!strcmp(cmd,"log")){
267 }else if (!strcmp(cmd,"ao")){
268 setAlphaOff(state,val);
271 else if (!strcmp(cmd,"udp_pos")){
272 startUdpPos(state,cmd2);
273 }else if (!strcmp(cmd,"udp_spd")){
274 startUdpSpd(state,cmd2);
284 * pocita procentualni odchylku od prumerneho proudu
286 static float diff_p(float value){
287 return ((float)value-PRUM_PROUD)*100/PRUM_PROUD;
290 * pocita procentualni odchylku od prumerneho souctu proudu
292 static float diff_s(float value){
293 return ((float)value-PRUM_SOUC)*100/PRUM_SOUC;
297 * tiskne potrebna data
299 void printData(struct rpi_state* state){
300 if (!doPrint) return;
302 struct rpi_in data_p;
303 struct rpi_state s; /*state*/
304 float cur0, cur1, cur2;
307 sem_wait(&state->thd_par_sem);
308 data_p = *state->spi_dat;
310 sem_post(&state->thd_par_sem);
312 if (data_p.adc_m_count){
313 cur0=data_p.ch0/data_p.adc_m_count;
314 cur1=data_p.ch1/data_p.adc_m_count;
315 cur2=data_p.ch2/data_p.adc_m_count;
317 for (i = 0; i < 16; i++) {
320 printf("%.2X ", data_p.debug_rx[i]);
323 printf("\npozice=%ld\n",data_p.pozice);
324 printf("rychlost=%d\n",s.speed);
325 printf("chtena pozice=%d\n",s.desired_pos);
326 printf("chtena rychlost=%d\n",s.desired_spd);
327 printf("alpha offset=%hu\n",s.alpha_offset);
328 printf("transfer count=%u\n",s.tf_count);
329 printf("raw_pozice=%u\n",data_p.pozice_raw);
330 printf("raw_pozice last12=%u\n",(data_p.pozice_raw&0x0FFF));
331 printf("index position=%u\n",data_p.index_position);
332 printf("distance to index=%u\n",s.index_dist);
333 printf("hal1=%d, hal2=%d, hal3=%d\n",data_p.hal1,data_p.hal2,data_p.hal3);
334 printf("en1=%d, en2=%d, en3=%d (Last sent)\n",!!(0x40&s.test),!!(0x20&s.test),!!(0x10&s.test));
335 printf("shdn1=%d, shdn2=%d, shdn3=%d (L.s.)\n",!!(0x08&s.test),!!(0x04&s.test),!!(0x02&s.test));
336 printf(" PWM1=%u PWM2=%u PWM3=%u\n",s.pwm1,s.pwm2,s.pwm3);
337 printf("T_PWM1=%u T_PWM2=%u T_PWM3=%u\n",s.t_pwm1,s.t_pwm2, s.t_pwm3);
338 printf("Pocet namerenych proudu=%u\n",data_p.adc_m_count);
339 printf("(pwm1) (ch1)=%d (avg=%4.0f) (%2.2f%%)\n",data_p.ch1,cur1,diff_p(cur1));
340 printf("(pwm2) (ch2)=%d (avg=%4.0f)(%2.2f%%)\n",data_p.ch2,cur2,diff_p(cur2));
341 printf("(pwm3) (ch0)=%d (avg=%4.0f)(%2.2f%%)\n",data_p.ch0,cur0,diff_p(cur0));
342 printf("soucet prumeru=%5.0f (%2.2f%%)\n",cur0+cur1+cur2,diff_s(cur0+cur1+cur2));
343 printf("duty=%d\n",s.duty);
344 if (s.index_ok) printf("index ok\n");
345 if (s.commutate) printf("commutation in progress\n");
346 if (s.doLogs) printf("logujeme\n");
347 if (s.error) printf("error pri maloc logs!! \n");