summaryrefslogtreecommitdiffstats
path: root/stm32/app/motor.c
diff options
context:
space:
mode:
Diffstat (limited to 'stm32/app/motor.c')
-rw-r--r--stm32/app/motor.c295
1 files changed, 155 insertions, 140 deletions
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
generated by cgit v1.2.3 (git 2.25.1) at 2024-05-10 00:49:26 +0000