diff options
Diffstat (limited to 'stm32/app/motor.c')
| -rw-r--r-- | stm32/app/motor.c | 72 |
1 files changed, 70 insertions, 2 deletions
diff --git a/stm32/app/motor.c b/stm32/app/motor.c index 392089b..8ffc5e6 100644 --- a/stm32/app/motor.c +++ b/stm32/app/motor.c @@ -3,9 +3,42 @@ static unsigned motor_pos[HANDS]; +#define M1_A GPIO11 +#define M1_A_PORT GPIOB +#define M1_B GPIO12 +#define M1_B_PORT GPIOB +#define M1_C GPIO13 +#define M1_C_PORT GPIOB +#define M1_D GPIO14 +#define M1_D_PORT GPIOB + static void coils (unsigned m, int a, int b, int c, int d) { + + if (!m) return; + + if (a) + SET (M1_A); + else + CLEAR (M1_A); + + if (b) + SET (M1_B); + else + CLEAR (M1_B); + + if (c) + SET (M1_C); + else + CLEAR (M1_C); + + if (d) + SET (M1_D); + else + CLEAR (M1_D); + + printf ("Motor %d: %+d %+d\r\n", m, a - b, c - d); } @@ -18,7 +51,26 @@ static void step (unsigned m, int d) s += d; s %= MOTOR_STEPS; - coils (m, !! ((s + 6) & 4), !! ((s + 1) & 4), !! ((s + 4) & 4), !! ((s + 7) & 4)); + + switch (s & 3) { + case 0: + coils (m, 0, 1, 0, 1); + break; + + case 1: + coils (m, 0, 1, 1, 0); + break; + + case 2: + coils (m, 1, 0, 1, 0); + break; + + case 3: + coils (m, 1, 0, 0, 1); + break; + } + + if (!m) BKP_DR8 = motor_pos[0] = s; @@ -36,11 +88,20 @@ void motor_tick (void) { static uint32_t last; unsigned i, d; + static unsigned wiggle = MOTOR_STEPS; + - if ((ticks - last) < 10) return; + + if ((ticks - last) < 4) return; last = ticks; + if (wiggle) { + step (1, 1); + wiggle--; + return; + } + if (!hands_ready) return; //printf("HANDS: %d -> %d %d -> %d\r\n",hands_pos[0],motor_pos[0],hands_pos[1],motor_pos[1]); @@ -51,6 +112,7 @@ void motor_tick (void) d -= motor_pos[i]; d %= MOTOR_STEPS; + if (d) { if (d < (MOTOR_STEPS / 2)) step (i, 1); @@ -66,10 +128,16 @@ void motor_tick (void) void motor_init (void) { + MAP_OUTPUT_PP (M1_A); + MAP_OUTPUT_PP (M1_B); + MAP_OUTPUT_PP (M1_C); + MAP_OUTPUT_PP (M1_D); + motor_pos[0] = BKP_DR8; motor_pos[1] = BKP_DR9; step (0, 0); step (1, 0); + } |