From 440039cb23f786573940ec1eafa665f9de561eae Mon Sep 17 00:00:00 2001
From: root <root@artemis.panaceas.org>
Date: Mon, 29 Nov 2021 12:40:33 +0000
Subject: fish

---
 stm32/app/motor.c | 171 ++++++++++++++++++++++++++++++++++++++++++++++--------
 1 file changed, 147 insertions(+), 24 deletions(-)

(limited to 'stm32/app/motor.c')

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);
 
-
 }
-- 
cgit v1.2.3