diff options
Diffstat (limited to 'stm32/app/motor.c')
| -rw-r--r-- | stm32/app/motor.c | 171 |
1 files changed, 147 insertions, 24 deletions
diff --git a/stm32/app/motor.c b/stm32/app/motor.c index ca9fa13..621812d 100644 --- a/stm32/app/motor.c +++ b/stm32/app/motor.c @@ -4,6 +4,8 @@ static unsigned motor_pos[HANDS]; static unsigned off[HANDS]; +#ifdef HBRIDGE + #define M1_A GPIO11 #define M1_A_PORT GPIOB #define M1_B GPIO12 @@ -13,13 +15,32 @@ static unsigned off[HANDS]; #define M1_D GPIO14 #define M1_D_PORT GPIOB +#else + +#define M1_DIR GPIO0 +#define M1_DIR_PORT GPIOB +#define M1_STEP GPIO1 +#define M1_STEP_PORT GPIOB +#define M1_EN GPIO2 +#define M1_EN_PORT GPIOB + +#define M2_DIR GPIO10 +#define M2_DIR_PORT GPIOC +#define M2_STEP GPIO11 +#define M2_STEP_PORT GPIOC +#define M2_EN GPIO12 +#define M2_EN_PORT GPIOC + + #define ADJUST_CCW GPIO0 #define ADJUST_CCW_PORT GPIOC #define ADJUST_CW GPIO1 #define ADJUST_CW_PORT GPIOC +#endif +#ifdef HBRIDGE static void coils (unsigned m, int a, int b, int c, int d) { @@ -63,17 +84,17 @@ b^=d; //printf ("Motor %d: %+d %+d\r\n", m, a - b, c - d); } +#endif static void step (unsigned m, int d) { unsigned s = motor_pos[m]; - if (d < 0) d += MOTOR_STEPS; - - s += d; + s += MOTOR_STEPS + d; s %= MOTOR_STEPS; +#ifdef HBRIDGE #if 0 // full step switch (s & 3) { @@ -123,6 +144,35 @@ break; } #endif +#else + +if (d<0) { +if (!m) { + SET(M1_DIR); + SET(M1_STEP); + delay_us(1); + CLEAR(M1_STEP); +} else { + SET(M2_DIR); + SET(M2_STEP); + delay_us(1); + CLEAR(M2_STEP); +} +} else if (d>0) { +if (!m) { + CLEAR(M1_DIR); + SET(M1_STEP); + delay_us(1); + CLEAR(M1_STEP); +} else { + CLEAR(M2_DIR); + SET(M2_STEP); + delay_us(1); + CLEAR(M2_STEP); +} +} + +#endif if (!m) @@ -135,47 +185,61 @@ break; -#define ADJ_P 2500 - void motor_tick (void) { static uint32_t last,blast; unsigned i, d; - static unsigned adjusting = ADJ_P; /*5 seconds*/ + static unsigned adjusting =0; + static unsigned zero=8000; unsigned hp[2]; + static unsigned s; - if ((ticks - last) < 2) return; - last=ticks; - if ((ticks - blast) > 100) { - if (!GET(ADJUST_CCW)) { - adjusting=ADJ_P; + //if ((ticks - last) < 1) return; + //last=ticks; - motor_pos[1]+=MOTOR_PHASES; - motor_pos[1]%=MOTOR_STEPS; + if ((ticks - blast) > 1000) { + + if (!GET(ADJUST_CCW)) { + adjusting=8000; } if (!GET(ADJUST_CW)) { - adjusting=ADJ_P; - - motor_pos[1]+=MOTOR_STEPS-MOTOR_PHASES; - motor_pos[1]%=MOTOR_STEPS; + zero=8000; } blast=ticks; - } - +#ifndef HBRIDGE + if (adjusting) { + adjusting--; + + if (adjusting) { + SET(M1_EN); + SET(M2_EN); + } else { + CLEAR(M1_EN); + CLEAR(M2_EN); + } + + motor_pos[0]=motor_pos[1]=0; + step(0,0); + step(1,0); + + return; + } +#endif + - if (adjusting || !hands_ready) { - if (adjusting) adjusting--; + if (zero || !hands_ready) { + if (zero) zero--; hp[0]=0; hp[1]=0; } else { @@ -186,6 +250,36 @@ void motor_tick (void) //printf("HANDS: %d -> %d %d -> %d\r\n",hands_pos[0],motor_pos[0],hands_pos[1],motor_pos[1]); +#if 0 +{ +unsigned static r=0; +r++; +r%=6000; + +if (r<3000) { +hp[0]=0; +} +} +#endif + +{ unsigned static s=0; + + +if (!s) { + printf("%4d %4d hp[0]=%6d mp[0]=%6d hp[1]=%6d mp[1]=%6d\r\n", + zero,adjusting, hp[0],motor_pos[0],hp[1],motor_pos[1]); +} + + + + +s++; +s%=500; +} + + + + for (i = 0; i < HANDS; ++i) { d = MOTOR_STEPS; d+=hp[i]; @@ -200,6 +294,9 @@ void motor_tick (void) step (i, -1); } } + + + } @@ -208,13 +305,40 @@ void motor_tick (void) void motor_init (void) { + MAP_INPUT_PU(ADJUST_CCW); + MAP_INPUT_PU(ADJUST_CW); + +#ifdef HBRIDGE MAP_OUTPUT_PP (M1_A); MAP_OUTPUT_PP (M1_B); MAP_OUTPUT_PP (M1_C); MAP_OUTPUT_PP (M1_D); - MAP_INPUT_PU(ADJUST_CCW); - MAP_INPUT_PU(ADJUST_CW); + +#else + + MAP_OUTPUT_PP (M1_EN); + MAP_OUTPUT_PP (M1_DIR); + MAP_OUTPUT_PP (M1_STEP); + + MAP_OUTPUT_PP (M2_EN); + MAP_OUTPUT_PP (M2_DIR); + MAP_OUTPUT_PP (M2_STEP); + + SET(M1_EN); + CLEAR(M1_STEP); + CLEAR(M1_DIR); + delay_us(2); + CLEAR(M1_EN); + + + SET(M2_EN); + CLEAR(M2_STEP); + CLEAR(M2_DIR); + delay_us(2); + CLEAR(M2_EN); +#endif + motor_pos[0] = BKP_DR8; @@ -223,5 +347,4 @@ void motor_init (void) step (0, 0); step (1, 0); - } |