From 6ea2c15dea720cbe6d633b7cf29d0a60bd2837ee Mon Sep 17 00:00:00 2001
From: Mats van Reenen <mail@matsvanreenen.nl>
Date: Mon, 15 Jun 2020 12:49:47 +0200
Subject: [PATCH] allow running motors on different speeds

---
 main.c | 167 ++++++++++++++++++++++++++++++++++++++++++---------------
 1 file changed, 123 insertions(+), 44 deletions(-)

diff --git a/main.c b/main.c
index a015f0e..1452eea 100644
--- a/main.c
+++ b/main.c
@@ -12,9 +12,9 @@
  * CS   <-> P1.3
  * 
  * motoren:
- * stapR <-> P1.6
+ * stepR <-> P1.6
  * dirR  <-> P2.5
- * stapL <-> P1.7
+ * stepL <-> P1.7
  * dirL  <-> P2.6
  * 
  * Lijndetectie:
@@ -32,33 +32,110 @@
  */
 #define TEST 0
 
-#define STEP_PINS  BIT4 | BIT5 // PORT 2
+#define STEPr_PIN  BIT4
+#define STEPl_PIN  BIT5 // PORT 2
 #define DIRr_PIN   BIT6 // PORT 1
 #define DIRl_PIN   BIT7 // PORT 1
-enum Direction {Forward=0, Backward=(DIRl_PIN | DIRr_PIN), RightTurn=DIRr_PIN, LeftTurn=DIRl_PIN};
+enum Direction {Forward=0, Backward=(DIRl_PIN | DIRr_PIN), rotateRight=DIRr_PIN, rotateLeft=DIRl_PIN};
+enum Speed {Fast=35, Normal=70, Slow=140, Stop=0}; // *10 us per step
 
-const uchar v = 36;
 const ulong halfRound = 2800;
 
-void stap(uint n, ulong t){
-  ulong d;
-  uint s = n-100;
+/* delay(): wait the given time
+ *
+ * @pram tus (ulong): time to wait in ten microsecons (tus * 10us)
+ */
+void delay(ulong tus){
+  while(tus != 0){
+    tus--;
+    __delay_cycles(160);
+  }
+}
+
+//TODO: add more steps in accareration
+/* move(): rotate the motors n steps on the fastest motor
+ *
+ * @pram n (uint): number of steps for the fastest motor
+ * @pram SR (enum Speed): speed for right motor
+ * @pram SL (enum Speed): speed for left motor
+ */
+void move(uint n, enum Speed SR, enum Speed SL){
+  enum Speed speed[2];
+  uchar start, fastest, slowest, fastPin, i;
+  uint dev;
+
+  speed[0] = SR;
+  speed[1] = SL;
+
+  // end of acceration (start on the fiven speed)
+  start = n - 100;
+
+  // get fastest speed
+  if(speed[0] > speed[1]){
+    fastest = 1;
+    slowest = 0;
+    fastPin = STEPl_PIN;
+  }else{
+    fastest = 0;
+    slowest = 1;
+    fastPin = STEPr_PIN;
+  }
+
+  if(speed[slowest] == Stop){
+    dev = 65535; // this is not quite stop but close enough
+  }else{
+    dev = speed[fastest]/speed[slowest];
+  }
+
+  if(speed[fastest] == Stop){
+    return; // fastest speed is stop, so dont need to run the motors
+  }
+
+  // acceleration
+  while(n > start && n != 0){
+    n--;
+
+    if(speed[slowest] == Stop){
+      P2OUT |= fastPin;
+      P2OUT &= ~fastPin;
+    }else{
+      P2OUT |= (STEPl_PIN | STEPr_PIN);
+      P2OUT &= ~(STEPl_PIN | STEPr_PIN);
+    }
+
+    delay(Slow);
+  }
+
+  // run on given speed
+  i = dev;
   while(n != 0){
     n--;
-    P2OUT |= STEP_PINS;
-    P2OUT &= ~STEP_PINS;
-    if(s < n || n < 100)
-        d = 100; // a crude way of a accaleration
-    else
-        d = t;
-    while(d != 0){
-        d--;
-         __delay_cycles(160);
+    i--;
+
+    if(i == 0){
+      P2OUT |= (STEPl_PIN | STEPr_PIN);
+      P2OUT &= ~(STEPl_PIN | STEPr_PIN);
+      i = dev;
+    }else{
+      P2OUT |= fastPin;
+      P2OUT &= ~fastPin;
     }
+
+    if(n == 100){ // start deaccaleration
+      speed[fastest] = Slow;
+      if(speed[slowest] != Stop){
+        dev = 1;
+      }
+    }
+    delay(speed[fastest]);
   }
   __delay_cycles(160000);
 }
 
+/* dir(): set the directon to rotatate the motors
+ *
+ * @pram d (enum Direction): [Forward | Backward | rotateRight | rotateLeft]
+ */
 void dir(enum Direction d){
   P1OUT &= ~(DIRr_PIN | DIRl_PIN); // set dir bits to 0
   P1OUT |= d;
@@ -78,7 +155,7 @@ int main(void)
     BCSCTL1 = CALBC1_16MHZ;
     DCOCTL  = CALDCO_16MHZ;
 
-    P2DIR |= STEP_PINS;
+    P2DIR |= STEPr_PIN | STEPl_PIN;
     P1DIR |= DIRr_PIN | DIRl_PIN;
 
     SPIInit();
@@ -89,36 +166,38 @@ int main(void)
     __delay_cycles(16000000);
 
     for(ii=2; ii>0; ii--){
-        dir(Forward);
-        stap(10000, v);
-        dir(RightTurn);
-        stap(halfRound, v*2);
-        dir(Forward);
-        stap(10000, v);
-        dir(LeftTurn);
-        stap(halfRound, v*2);
+      // forward
+      dir(Forward);
+      move(10000, Fast, Fast);
+      dir(rotateRight);
+      move(halfRound, Normal, Normal);
+      // return
+      dir(Forward);
+      move(10000, Fast, Fast);
+      dir(rotateLeft);
+      move(halfRound, Normal, Normal);
     }
     for(ii=2; ii>0; ii--){
-        for(i=5; i>0; i--){
-            dir(Forward);
-            stap(1600, v);
-            dir(RightTurn);
-            stap(halfRound/2, v*2);
-        }
+      for(i=5; i>0; i--){
         dir(Forward);
-        stap(1600, v);
-        dir(RightTurn);
-        stap(halfRound, v*2);
-        for(i=5; i>0; i--){
-            dir(Forward);
-            stap(1600, v);
-            dir(LeftTurn);
-            stap(halfRound/2, v*2);
-        }
+        move(1600, Fast, Fast);
+        dir(rotateRight);
+        move(halfRound/2, Normal, Normal);
+      }
+      dir(Forward);
+      move(1600, Fast, Fast);
+      dir(rotateRight);
+      move(halfRound, Normal, Normal);
+      for(i=5; i>0; i--){
         dir(Forward);
-        stap(1600, v);
-        dir(LeftTurn);
-        stap(halfRound, v*2);
+        move(1600, Fast, Fast);
+        dir(rotateLeft);
+        move(halfRound/2, Normal, Normal);
+      }
+      dir(Forward);
+      move(1600, Fast, Fast);
+      dir(rotateLeft);
+      move(halfRound, Normal, Normal);
     }
 
     return 0;