From 869118f74612829db47d0d681cee5b4af937b16f Mon Sep 17 00:00:00 2001 From: root Date: Tue, 30 Nov 2021 00:24:43 +0000 Subject: tmc2209 first cut --- stm32/app/motor.c | 295 ++++++++++++++++++++++++++++-------------------------- 1 file changed, 155 insertions(+), 140 deletions(-) (limited to 'stm32/app/motor.c') diff --git a/stm32/app/motor.c b/stm32/app/motor.c index 621812d..b599896 100644 --- a/stm32/app/motor.c +++ b/stm32/app/motor.c @@ -2,7 +2,7 @@ static unsigned motor_pos[HANDS]; -static unsigned off[HANDS]; +//static unsigned off[HANDS]; #ifdef HBRIDGE @@ -17,25 +17,25 @@ static unsigned off[HANDS]; #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 +#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 @@ -46,18 +46,18 @@ static void coils (unsigned m, int a, int b, int c, int d) if (!m) return; - c^=d; - d^=c; - c^=d; + c ^= d; + d ^= c; + c ^= d; #if 0 -a^=c; -c^=a; -a^=c; + a ^= c; + c ^= a; + a ^= c; -b^=d; -d^=b; -b^=d; + b ^= d; + d ^= b; + b ^= d; #endif @@ -96,6 +96,7 @@ static void step (unsigned m, int d) #ifdef HBRIDGE #if 0 + // full step switch (s & 3) { case 0: @@ -114,63 +115,70 @@ 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)); + // 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; -} + //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 #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); -} -} + 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 @@ -188,13 +196,12 @@ if (!m) { void motor_tick (void) { - static uint32_t last,blast; + static uint32_t blast; unsigned i, d; - static unsigned adjusting =0; - static unsigned zero=8000; + static unsigned adjusting = 0; + static unsigned zero = 8000; unsigned hp[2]; - static unsigned s; @@ -205,84 +212,86 @@ void motor_tick (void) if ((ticks - blast) > 1000) { - if (!GET(ADJUST_CCW)) { - adjusting=8000; - } + if (!GET (ADJUST_CCW)) + adjusting = 8000; - if (!GET(ADJUST_CW)) { - zero=8000; - } - blast=ticks; + if (!GET (ADJUST_CW)) + 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; + + 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 (zero || !hands_ready) { - if (zero) zero--; - hp[0]=0; - hp[1]=0; + if (zero) zero--; + + hp[0] = 0; + hp[1] = 0; } else { - hp[0]=hands_pos[0]; - hp[1]=hands_pos[1]; + 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]); #if 0 -{ -unsigned static r=0; -r++; -r%=6000; + { + unsigned static r = 0; + r++; + r %= 6000; -if (r<3000) { -hp[0]=0; -} -} + if (r < 3000) + hp[0] = 0; + } #endif -{ unsigned static s=0; + { + static unsigned 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]); -} + 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; -} + s++; + s %= 500; + } for (i = 0; i < HANDS; ++i) { d = MOTOR_STEPS; - d+=hp[i]; + d += hp[i]; d -= motor_pos[i]; d %= MOTOR_STEPS; @@ -305,8 +314,8 @@ s%=500; void motor_init (void) { - MAP_INPUT_PU(ADJUST_CCW); - MAP_INPUT_PU(ADJUST_CW); + MAP_INPUT_PU (ADJUST_CCW); + MAP_INPUT_PU (ADJUST_CW); #ifdef HBRIDGE MAP_OUTPUT_PP (M1_A); @@ -325,18 +334,24 @@ void motor_init (void) 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 (M1_EN); + CLEAR (M1_STEP); + CLEAR (M1_DIR); + + + SET (M2_EN); + CLEAR (M2_STEP); + CLEAR (M2_DIR); + + + if (tmc2209_init()) { + printf ("Motor controllers failed to init, not enabling drive\n"); + return; + } + CLEAR (M1_EN); + CLEAR (M2_EN); - SET(M2_EN); - CLEAR(M2_STEP); - CLEAR(M2_DIR); - delay_us(2); - CLEAR(M2_EN); #endif -- cgit v1.2.3