diff options
Diffstat (limited to 'stm32/app/motor.c')
-rw-r--r-- | stm32/app/motor.c | 106 |
1 files changed, 95 insertions, 11 deletions
diff --git a/stm32/app/motor.c b/stm32/app/motor.c index 8ffc5e6..ca9fa13 100644 --- a/stm32/app/motor.c +++ b/stm32/app/motor.c @@ -2,6 +2,7 @@ static unsigned motor_pos[HANDS]; +static unsigned off[HANDS]; #define M1_A GPIO11 #define M1_A_PORT GPIOB @@ -12,12 +13,33 @@ static unsigned motor_pos[HANDS]; #define M1_D GPIO14 #define M1_D_PORT GPIOB +#define ADJUST_CCW GPIO0 +#define ADJUST_CCW_PORT GPIOC +#define ADJUST_CW GPIO1 +#define ADJUST_CW_PORT GPIOC + + static void coils (unsigned m, int a, int b, int c, int d) { if (!m) return; + c^=d; + d^=c; + c^=d; + +#if 0 +a^=c; +c^=a; +a^=c; + +b^=d; +d^=b; +b^=d; +#endif + + if (a) SET (M1_A); else @@ -39,7 +61,7 @@ static void coils (unsigned m, int a, int b, int c, int d) CLEAR (M1_D); - printf ("Motor %d: %+d %+d\r\n", m, a - b, c - d); + //printf ("Motor %d: %+d %+d\r\n", m, a - b, c - d); } static void step (unsigned m, int d) @@ -52,6 +74,8 @@ static void step (unsigned m, int d) s %= MOTOR_STEPS; +#if 0 + // full step switch (s & 3) { case 0: coils (m, 0, 1, 0, 1); @@ -69,6 +93,35 @@ static void step (unsigned m, int d) coils (m, 1, 0, 0, 1); break; } +#endif +#if 0 +// half step +coils (m, !! ((s + 1) & 4), !! ((s + 6) & 4), !! ((s + 4) & 4), !! ((s + 7) & 4)); +#endif +#if 1 +//60deg phase + +switch(s%6) { +case 0: +coils(m,0,0,1,0); +break; +case 1: +coils(m,1,0,1,0); +break; +case 2: +coils(m,1,0,0,0); +break; +case 3: +coils(m,0,0,0,1); +break; +case 4: +coils(m,0,1,0,1); +break; +case 5: +coils(m,0,1,0,0); +break; +} +#endif @@ -82,33 +135,60 @@ static void step (unsigned m, int d) +#define ADJ_P 2500 void motor_tick (void) { - static uint32_t last; + static uint32_t last,blast; unsigned i, d; - static unsigned wiggle = MOTOR_STEPS; + static unsigned adjusting = ADJ_P; /*5 seconds*/ + unsigned hp[2]; + + if ((ticks - last) < 2) return; + last=ticks; - if ((ticks - last) < 4) return; - last = ticks; + if ((ticks - blast) > 100) { - if (wiggle) { - step (1, 1); - wiggle--; - return; + if (!GET(ADJUST_CCW)) { + adjusting=ADJ_P; + + motor_pos[1]+=MOTOR_PHASES; + motor_pos[1]%=MOTOR_STEPS; } - if (!hands_ready) return; + + if (!GET(ADJUST_CW)) { + adjusting=ADJ_P; + + motor_pos[1]+=MOTOR_STEPS-MOTOR_PHASES; + motor_pos[1]%=MOTOR_STEPS; + } + blast=ticks; + + } + + + + + if (adjusting || !hands_ready) { + if (adjusting) adjusting--; + hp[0]=0; + hp[1]=0; + } else { + hp[0]=hands_pos[0]; + hp[1]=hands_pos[1]; + } //printf("HANDS: %d -> %d %d -> %d\r\n",hands_pos[0],motor_pos[0],hands_pos[1],motor_pos[1]); for (i = 0; i < HANDS; ++i) { - d = MOTOR_STEPS + hands_pos[i]; + d = MOTOR_STEPS; + d+=hp[i]; d -= motor_pos[i]; d %= MOTOR_STEPS; @@ -133,6 +213,10 @@ void motor_init (void) MAP_OUTPUT_PP (M1_C); MAP_OUTPUT_PP (M1_D); + MAP_INPUT_PU(ADJUST_CCW); + MAP_INPUT_PU(ADJUST_CW); + + motor_pos[0] = BKP_DR8; motor_pos[1] = BKP_DR9; |