From 6a4fcd4194499752f6f657f8af8940af5b22a212 Mon Sep 17 00:00:00 2001
From: sasha <sasha@cirno.(none)>
Date: Sat, 29 Mar 2014 23:31:53 +0200
Subject: [PATCH] Add Stepper class

---
 Arduino/arduino.py               | 40 ++++++++++++++++++++
 examples.py                      | 17 ++++++++-
 sketches/prototype/prototype.ino | 63 ++++++++++++++++++++++++++++++++
 3 files changed, 119 insertions(+), 1 deletion(-)

diff --git a/Arduino/arduino.py b/Arduino/arduino.py
index 58bc020..4cfb378 100755
--- a/Arduino/arduino.py
+++ b/Arduino/arduino.py
@@ -110,6 +110,7 @@ def __init__(self, baud=9600, port=None, timeout=2, sr=None):
         self.sr = sr
         self.SoftwareSerial = SoftwareSerial(self)
         self.Servos = Servos(self)
+        self.Stepper = Stepper(self)
 
     def version(self):
         return get_version(self.sr)
@@ -423,6 +424,45 @@ def __init__(self, board):
         self.board = board
         self.sr = board.sr
 
+class Stepper(object):
+
+    """
+    Class for Arduino stepper support
+    """
+
+    def __init__(self, board):
+        self.board = board
+        self.sr = board.sr
+        self.stepper_pos = {}
+
+    def attach(self, steps, pin1=0, pin2=0, pin3=0, pin4=0):
+        cmd_str = build_cmd_str("spit", (steps, pin1, pin2, pin3, pin4))
+        while True:
+            self.sr.write(cmd_str)
+            self.sr.flush()
+
+            rd = self.sr.readline().replace("\r\n", "")
+            if rd:
+                break
+            else:
+                log.debug("trying to attach servo to pin {0}".format(pin1))
+        position = int(rd)
+        self.stepper_pos[pin1] = position
+        return 1
+
+    def step(self, pin, steps):
+        position = self.stepper_pos[pin]
+        cmd_str = build_cmd_str("spsp", (position, steps))
+
+        self.sr.write(cmd_str)
+        self.sr.flush()
+
+    def speed(self, pin, speed):
+        position = self.stepper_pos[pin]
+        cmd_str = build_cmd_str("spsd", (position, speed))
+
+        self.sr.write(cmd_str)
+        self.sr.flush()
 
 class Servos(object):
 
diff --git a/examples.py b/examples.py
index 5a2e143..779d43d 100644
--- a/examples.py
+++ b/examples.py
@@ -72,5 +72,20 @@ def LCD(tx, baud, ssbaud, message, port=""):
     while True:
         board.SoftwareSerial.write(" test ")
 
+def Stepper(baud, port, steps, pin1, pin2, pin3=0, pin4=0, speed=120):
+    """
+    Steps
+    """
+    board = Arduino(baud, port=port)
+    board.pinMode(13, "OUTPUT")
+    board.Stepper.attach(steps,pin1, pin2, pin3, pin4)
+    board.Stepper.speed(pin1, speed)
+    while True:
+        board.Stepper.step(pin1, steps)
+        time.sleep(1)
+        board.Stepper.step(pin1, -steps)
+        time.sleep(1)
+
 if __name__ == "__main__":
-    Blink(13, '9600')
+    #Blink(13, '9600')
+    Stepper('9600',"/dev/ttyUSB0", 100, 9, 10)
diff --git a/sketches/prototype/prototype.ino b/sketches/prototype/prototype.ino
index 867c360..250bf25 100644
--- a/sketches/prototype/prototype.ino
+++ b/sketches/prototype/prototype.ino
@@ -1,10 +1,14 @@
 #include <SoftwareSerial.h>
 #include <Wire.h>
 #include <Servo.h>
+#include <Stepper.h>
 
 SoftwareSerial *sserial = NULL;
 Servo servos[8];
+Stepper steppers[4];
+
 int servo_pins[] = {0, 0, 0, 0, 0, 0, 0, 0};
+int stepper_pins[] = {0, 0, 0, 0};
 boolean connected = false;
 
 int Str2int (String Str_value)
@@ -301,6 +305,56 @@ void SV_write_ms(String data) {
     servos[pos].writeMicroseconds(uS);
 }
 
+void SP_init(String data) {
+    String sdata[5];
+    split(sdata,5,data,'%');
+    int steps = Str2int(sdata[0]);
+    int pin1 = Str2int(sdata[1]);
+    int pin2 = Str2int(sdata[2]); 
+    int pin3 = Str2int(sdata[3]);
+    int pin4 = Str2int(sdata[4]); 
+    /*int pos = -1;
+    for (int i = 0; i<4;i++) {
+        if (stepper_pins[i] == pin1) { //reset in place
+            if(pin4>0)
+                steppers[pos].attach(steps, pin1, pin2, pin3, pin4);
+            else
+                steppers[pos].attach(steps, pin1, pin2);
+            stepper_pins[pos] = pin1;
+            Serial.println(pos);
+            return;
+            }
+        }
+    for (int i = 0; i<4;i++) {
+        if (servo_pins[i] == 0) {pos = i;break;} // find spot in stepper array
+        }
+    if (pos == -1) {;} //no array position available!
+    else {*/
+    int pos=1;
+        if(pin4>0)
+            steppers[pos].attach(steps, pin1, pin2, pin3, pin4);
+        else
+            steppers[pos].attach(steps, pin1, pin2);
+        stepper_pins[pos] = pin1;
+        Serial.println(pos);
+        //}  
+}
+
+void SP_speed(String data) {
+    String sdata[2];
+    split(sdata,2,data,'%');
+    int pos = Str2int(sdata[0]);
+    int speed = Str2int(sdata[1]);
+    steppers[pos].setSpeed(speed);
+}
+
+void SP_step(String data) {
+    String sdata[2];
+    split(sdata,2,data,'%');
+    int pos = Str2int(sdata[0]);
+    int steps = Str2int(sdata[1]);
+    steppers[pos].step(steps);
+}
 
 void SerialParser(void) {
   char readChar[64];
@@ -377,6 +431,15 @@ void SerialParser(void) {
   else if (cmd == "si") {
       shiftInHandler(data);
   }
+  else if (cmd == "spit") {
+      SP_init(data);   
+  } 
+  else if (cmd == "spsp") {
+      SP_step(data);   
+  }       
+  else if (cmd == "spsd") {
+      SP_speed(data);   
+  }       
 
 }