#include "rpin.h" /*gpclk*/
#include "rp_spi.h" /*spi*/
#include "misc.h" /*structure for priorities*/
+#include "pxmc_sin_fixed.h" /*to test sin commutation */
+
#define PRUM_PROUD 2061
#define THREAD_SHARED 0
#define INIT_VALUE 0 /*init value for semaphor*/
+
+#define PXMC_SIN_FIX_TAB_BITS 9
+#define PXMC_SIN_FIX_IDX_SLR 23
+#define PXMC_SIN_FIX_XD_MASK 0x007fffff
+#define PXMC_SIN_FIX_XD_SLR 8
+#define PXMC_SIN_FIX_A_MASK 0xffffc000
+#define PXMC_SIN_FIX_B_SLL 19
+#define PXMC_SIN_FIX_B_SAR 16
+#define PXMC_SIN_FIX_B_XD_SAR 6
+#define PXMC_SIN_FIX_ZIC_MASK 0x00002000
+#define PXMC_SIN_FIX_ZIC_BIT 13
+
+#define PXMC_SIN_FIX_PI2 0x40000000
+#define PXMC_SIN_FIX_2PI3 0x55555555
+
struct sigaction sighnd; /*struktura pro signal handler*/
struct rpi_in data;
struct rpi_state{
uint8_t test;
uint16_t pwm1, pwm2, pwm3;
+ uint16_t t_pwm1, t_pwm2, t_pwm3;
char commutate;
int duty; /* duty cycle of pwm */
uint16_t index_dist; /* distance to index position */
printf("raw_pozice=%d\n",(int32_t)data_p.pozice_raw);
printf("raw_pozice last12=%u\n",(data_p.pozice_raw&0x0FFF));
printf("index position=%u\n",data_p.index_position);
- printf("distance to index=%u\n",s.index_dist);
printf("hal1=%d, hal2=%d, hal3=%d\n",data_p.hal1,data_p.hal2,data_p.hal3);
printf("en1=%d, en2=%d, en3=%d (Last sent)\n",!!(0x40&s.test),!!(0x20&s.test),!!(0x10&s.test));
printf("shdn1=%d, shdn2=%d, shdn3=%d (L.s.)\n",!!(0x08&s.test),!!(0x04&s.test),!!(0x02&s.test));
printf("PWM1=%u(L.s.)\n",s.pwm1);
printf("PWM2=%u(L.s.)\n",s.pwm2);
printf("PWM3=%u(L.s.)\n",s.pwm3);
+ printf("distance to index=%u\n",s.index_dist);
+ printf("T_PWM1=%u T_PWM2=%u T_PWM3=%u\n",s.t_pwm1,s.t_pwm2, s.t_pwm3);
printf("Pocet namerenych proudu=%u\n",data_p.adc_m_count);
printf("(pwm1) (ch1)=%d (avg=%4.0f) (%2.2f%%)\n",data_p.ch1,cur1,diff_p(cur1));
printf("(pwm2) (ch2)=%d (avg=%4.0f)(%2.2f%%)\n",data_p.ch2,cur2,diff_p(cur2));
uint16_t tmp;
/* keep the cap*/
+
if (rps.pwm1>2047) rps.pwm1=2047;
if (rps.pwm2>2047) rps.pwm2=2047;
if (rps.pwm3>2047) rps.pwm3=2047;
}
return (void*)0;
}
+
+inline
+int sin_commutator(int duty){
+ #define DEGREE_60 715827883
+ #define DEGREE_120 1431655765
+ #define DEGREE_180 2147483648
+ #define DEGREE_240 2863311531
+ #define DEGREE_300 3579139413
+ uint32_t j,pos;
+ int32_t sin;
+ uint16_t pwm;
+ pos=rps.index_dist*4294967;
+ if (duty>=0){ /*clockwise rotation*/
+ /* 1st phase */
+ sin = pxmc_sin_fixed_inline(pos+DEGREE_240,10); /*10+1 bity*/ /*-120*/
+ pwm=0;
+ for(j=0;j!=11;j++){
+ /* multiplicate as if maximum sinus value was unity */
+ pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j));
+ }
+ rps.pwm1=pwm;
+
+ /* 2nd phase */
+ sin = pxmc_sin_fixed_inline(pos+DEGREE_120,10); /*10+1 bity*/ /*-240*/
+ pwm=0;
+ for(j=0;j!=11;j++){
+ /* multiplicate as if maximum sinus value was unity */
+ pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j));
+ }
+ rps.pwm2=pwm;
+
+ /* 3rd phase */
+ sin = pxmc_sin_fixed_inline(pos,10); /*10+1 bity*/
+ pwm=0;
+ for(j=0;j!=11;j++){
+ /* multiplicate as if maximum sinus value was unity */
+ pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j));
+ }
+ rps.pwm3=pwm;
+ }else{
+ duty=-duty;
+
+ /* 1st phase */
+ sin = pxmc_sin_fixed_inline(pos+DEGREE_60,10); /*10+1 bity*/ /*-300*/
+ pwm=0;
+ for(j=0;j!=11;j++){
+ /* multiplicate as if maximum sinus value was unity */
+ pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j));
+ }
+ rps.pwm1=pwm;
+
+ /* 2nd phase */
+ sin = pxmc_sin_fixed_inline(pos+DEGREE_300,10); /*10+1 bity*/ /*-60-*/
+ pwm=0;
+ for(j=0;j!=11;j++){
+ /* multiplicate as if maximum sinus value was unity */
+ pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j));
+ }
+ rps.pwm2=pwm;
+
+ /* 3rd phase */
+ sin = pxmc_sin_fixed_inline(pos+DEGREE_180,10); /*10+1 bity*/ /*-180*/
+ pwm=0;
+ for(j=0;j!=11;j++){
+ /* multiplicate as if maximum sinus value was unity */
+ pwm+=(!(sin & 0x10000000))*(((1 << j) & sin)>>j)*(duty>>(10-j));
+ }
+ rps.pwm3=pwm;
+ }
+ return 0;
+}
/*
* \brief
* Test function to be placed in controll loop.
* Switches PWM's at point where they produce same force.
* This points are found thanks to IRC position,
*/
-inline void simple_ind_dist_commutator(int duty){
+inline
+void simple_ind_dist_commutator(int duty){
if (duty>=0){ /* clockwise - so that position increase */
/* pwm3 */
if ((rps.index_dist>=45 && rps.index_dist<=373) ||
}
pid();
if (rps.index_ok && rps.commutate){
- simple_ind_dist_commutator(rps.duty);
+ /*simple_ind_dist_commutator(rps.duty);*/
+ sin_commutator(rps.duty);
}else if(!rps.index_ok && rps.commutate){
simple_hall_commutator(rps.duty);
}