From 0fa30aeb62b6b777d70df00c03b367f76f44bed6 Mon Sep 17 00:00:00 2001 From: Aniket Date: Sat, 2 Dec 2017 14:01:07 +0800 Subject: [PATCH 1/3] Adding stepper motor module --- drivers/gpio/stepper_driver.go | 230 ++++++++++++++++++++++++++++ drivers/gpio/stepper_driver_test.go | 105 +++++++++++++ 2 files changed, 335 insertions(+) create mode 100644 drivers/gpio/stepper_driver.go create mode 100644 drivers/gpio/stepper_driver_test.go diff --git a/drivers/gpio/stepper_driver.go b/drivers/gpio/stepper_driver.go new file mode 100644 index 00000000..e3af914a --- /dev/null +++ b/drivers/gpio/stepper_driver.go @@ -0,0 +1,230 @@ +package gpio + +import ( + "errors" + "math" + "strings" + "sync" + "time" + + "gobot.io/x/gobot" +) + +type phase [][4]byte + +// StepperModes to decide on Phase and Stepping +var StepperModes = struct { + SinglePhaseStepping [][4]byte + DualPhaseStepping [][4]byte + HalfStepping [][4]byte +}{ + //1 cycle = 4 steps with lesser torque + SinglePhaseStepping: [][4]byte{ + {1, 0, 0, 0}, + {0, 1, 0, 0}, + {0, 0, 1, 0}, + {0, 0, 0, 1}, + }, + //1 cycle = 4 steps with higher torque and current + DualPhaseStepping: [][4]byte{ + {1, 0, 0, 1}, + {1, 1, 0, 0}, + {0, 1, 1, 0}, + {0, 0, 1, 1}, + }, + //1 cycle = 8 steps with lesser torque than full stepping + HalfStepping: [][4]byte{ + {1, 0, 0, 1}, + {1, 0, 0, 0}, + {1, 1, 0, 0}, + {0, 1, 0, 0}, + {0, 1, 1, 0}, + {0, 0, 1, 0}, + {0, 0, 1, 1}, + {0, 0, 0, 1}, + }, +} + +// StepperDriver object +type StepperDriver struct { + name string + pins [4]string + connection DigitalWriter + phase phase + stepsPerRev uint + moving bool + direction string + stepNum int + speed uint + mutex *sync.Mutex +} + +// NewStepperDriver returns a new StepperDriver given a +// DigitalWriter +// Pins - To which the stepper is connected +// Phase - Defined by StepperModes {SinglePhaseStepping, DualPhaseStepping, HalfStepping} +// Steps - No of steps per revolution of Stepper motor +func NewStepperDriver(a DigitalWriter, pins [4]string, phase phase, stepsPerRev uint) *StepperDriver { + s := &StepperDriver{ + name: gobot.DefaultName("Stepper"), + connection: a, + pins: pins, + phase: phase, + stepsPerRev: stepsPerRev, + moving: false, + direction: "forward", + stepNum: 0, + speed: 1, + mutex: &sync.Mutex{}, + } + + s.speed = s.GetMaxSpeed() + return s +} + +// Name of StepperDriver +func (s *StepperDriver) Name() string { return s.name } + +// SetName sets name for StepperDriver +func (s *StepperDriver) SetName(n string) { s.name = n } + +// Connection returns StepperDriver's connection +func (s *StepperDriver) Connection() gobot.Connection { return s.connection.(gobot.Connection) } + +// Start implements the Driver interface and keeps running the stepper till halt is called +func (s *StepperDriver) Start() (err error) { return } + +// Run continuously runs the stepper +func (s *StepperDriver) Run() (err error) { + //halt if already moving + if s.moving == true { + s.Halt() + } + + s.mutex.Lock() + s.moving = true + s.mutex.Unlock() + + go func() { + for { + if s.moving == false { + break + } + s.step() + } + }() + + return +} + +// Halt implements the Driver interface and halts the motion of the Stepper +func (s *StepperDriver) Halt() (err error) { + s.mutex.Lock() + s.moving = false + s.mutex.Unlock() + return nil +} + +// SetDirection sets the direction in which motor should be moving, Default is forward +func (s *StepperDriver) SetDirection(direction string) error { + direction = strings.ToLower(direction) + if direction != "forward" && direction != "backward" { + return errors.New("Invalid direction. Value should be forward or backward") + } + + s.mutex.Lock() + s.direction = direction + s.mutex.Unlock() + return nil +} + +// IsMoving returns a bool stating whether motor is currently in motion +func (s *StepperDriver) IsMoving() bool { + return s.moving +} + +// Step moves motor one step in giving direction +func (s *StepperDriver) step() error { + if s.direction == "forward" { + s.stepNum++ + } else { + s.stepNum-- + } + + if s.stepNum >= int(s.stepsPerRev) { + s.stepNum = 0 + } else if s.stepNum < 0 { + s.stepNum = int(s.stepsPerRev) - 1 + } + + r := int(math.Abs(float64(s.stepNum))) % len(s.phase) + + for i, v := range s.phase[r] { + if err := s.connection.DigitalWrite(s.pins[i], v); err != nil { + return err + } + } + + return nil +} + +// Move moves the motor for given number of steps +func (s *StepperDriver) Move(stepsToMove int) error { + if stepsToMove == 0 { + return s.Halt() + } + + if s.moving == true { + //stop previous motion + s.Halt() + } + + s.mutex.Lock() + s.moving = true + s.direction = "forward" + + if stepsToMove < 0 { + s.direction = "backward" + } + defer s.mutex.Unlock() + + stepsLeft := int64(math.Abs(float64(stepsToMove))) + delay := time.Duration(60000*1000/(s.stepsPerRev*s.speed)) * time.Microsecond + + for stepsLeft > 0 { + if err := s.step(); err != nil { + return err + } + stepsLeft-- + time.Sleep(delay) //adjust accordingly + } + + s.moving = false + return nil +} + +// GetCurrentStep gives the current step of motor +func (s *StepperDriver) GetCurrentStep() int { + return s.stepNum +} + +// GetMaxSpeed gives the max RPM of motor +func (s *StepperDriver) GetMaxSpeed() uint { + //considering time for 1 rev as no of steps per rev * 1.5 (min time req between each step) + return uint(60000 / (float64(s.stepsPerRev) * 1.5)) +} + +// SetSpeed sets the rpm +func (s *StepperDriver) SetSpeed(rpm uint) error { + if rpm <= 0 { + return errors.New("RPM cannot be a negative value") + } + + m := s.GetMaxSpeed() + if rpm > m { + rpm = m + } + + s.speed = rpm + return nil +} diff --git a/drivers/gpio/stepper_driver_test.go b/drivers/gpio/stepper_driver_test.go new file mode 100644 index 00000000..a894a2c6 --- /dev/null +++ b/drivers/gpio/stepper_driver_test.go @@ -0,0 +1,105 @@ +package gpio + +import ( + "errors" + "strings" + "testing" + "time" + + "gobot.io/x/gobot/gobottest" +) + +const ( + stepsInRev = 32 +) + +func initStepperMotorDriver() *StepperDriver { + return NewStepperDriver(newGpioTestAdaptor(), [4]string{"7", "11", "13", "15"}, StepperModes.DualPhaseStepping, stepsInRev) +} + +func TestStepperDriverRun(t *testing.T) { + d := initStepperMotorDriver() + d.Run() + gobottest.Assert(t, d.IsMoving(), true) +} + +func TestStepperDriverHalt(t *testing.T) { + d := initStepperMotorDriver() + d.Run() + time.Sleep(200 * time.Millisecond) + d.Halt() + gobottest.Assert(t, d.IsMoving(), false) +} + +func TestStepperDriverDefaultName(t *testing.T) { + d := initStepperMotorDriver() + gobottest.Assert(t, strings.HasPrefix(d.Name(), "Stepper"), true) +} + +func TestStepperDriverSetName(t *testing.T) { + name := "SomeStepperSriver" + d := initStepperMotorDriver() + d.SetName(name) + gobottest.Assert(t, d.Name(), name) +} + +func TestStepperDriverSetDirection(t *testing.T) { + dir := "backward" + d := initStepperMotorDriver() + d.SetDirection(dir) + gobottest.Assert(t, d.direction, dir) +} + +func TestStepperDriverDefaultDirection(t *testing.T) { + d := initStepperMotorDriver() + gobottest.Assert(t, d.direction, "forward") +} + +func TestStepperDriverInvalidDirection(t *testing.T) { + d := initStepperMotorDriver() + err := d.SetDirection("reverse") + gobottest.Assert(t, err.(error), errors.New("Invalid direction. Value should be forward or backward")) +} + +func TestStepperDriverMoveForward(t *testing.T) { + d := initStepperMotorDriver() + d.Move(1) + gobottest.Assert(t, d.GetCurrentStep(), 1) + + d.Move(10) + gobottest.Assert(t, d.GetCurrentStep(), 11) +} + +func TestStepperDriverMoveBackward(t *testing.T) { + d := initStepperMotorDriver() + d.Move(-1) + gobottest.Assert(t, d.GetCurrentStep(), stepsInRev-1) + + d.Move(-10) + gobottest.Assert(t, d.GetCurrentStep(), stepsInRev-11) +} + +func TestStepperDriverMoveFullRotation(t *testing.T) { + d := initStepperMotorDriver() + d.Move(stepsInRev) + gobottest.Assert(t, d.GetCurrentStep(), 0) +} + +func TestStepperDriverMotorSetSpeedMoreThanMax(t *testing.T) { + d := initStepperMotorDriver() + m := d.GetMaxSpeed() + + d.SetSpeed(m + 1) + gobottest.Assert(t, m, d.speed) +} + +func TestStepperDriverMotorSetSpeedLessOrEqualMax(t *testing.T) { + d := initStepperMotorDriver() + m := d.GetMaxSpeed() + + d.SetSpeed(m - 1) + gobottest.Assert(t, m-1, d.speed) + + d.SetSpeed(m) + gobottest.Assert(t, m, d.speed) +} From 10925804ae8efd643532da5c7df7437e1838d317 Mon Sep 17 00:00:00 2001 From: Aniket Date: Sat, 2 Dec 2017 17:39:04 +0800 Subject: [PATCH 2/3] some refactoring --- drivers/gpio/stepper_driver.go | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/drivers/gpio/stepper_driver.go b/drivers/gpio/stepper_driver.go index e3af914a..dfb6896b 100644 --- a/drivers/gpio/stepper_driver.go +++ b/drivers/gpio/stepper_driver.go @@ -186,9 +186,10 @@ func (s *StepperDriver) Move(stepsToMove int) error { if stepsToMove < 0 { s.direction = "backward" } - defer s.mutex.Unlock() + s.mutex.Unlock() stepsLeft := int64(math.Abs(float64(stepsToMove))) + //Do not remove *1000 and change duration to time.Millisecond. It has been done for a reason delay := time.Duration(60000*1000/(s.stepsPerRev*s.speed)) * time.Microsecond for stepsLeft > 0 { @@ -196,7 +197,7 @@ func (s *StepperDriver) Move(stepsToMove int) error { return err } stepsLeft-- - time.Sleep(delay) //adjust accordingly + time.Sleep(delay) } s.moving = false @@ -217,7 +218,7 @@ func (s *StepperDriver) GetMaxSpeed() uint { // SetSpeed sets the rpm func (s *StepperDriver) SetSpeed(rpm uint) error { if rpm <= 0 { - return errors.New("RPM cannot be a negative value") + return errors.New("RPM cannot be a zero or negative value") } m := s.GetMaxSpeed() From 210a0e930bdd475f5d2b4f0b4220ae2f4bf4a219 Mon Sep 17 00:00:00 2001 From: Aniket Date: Sat, 2 Dec 2017 21:22:41 +0800 Subject: [PATCH 3/3] adding example for stepper motor --- examples/raspi_stepper_move.go | 37 ++++++++++++++++++++++++++++++++++ 1 file changed, 37 insertions(+) create mode 100644 examples/raspi_stepper_move.go diff --git a/examples/raspi_stepper_move.go b/examples/raspi_stepper_move.go new file mode 100644 index 00000000..1e61c7c4 --- /dev/null +++ b/examples/raspi_stepper_move.go @@ -0,0 +1,37 @@ +package main + +import ( + "fmt" + + "gobot.io/x/gobot" + "gobot.io/x/gobot/drivers/gpio" + "gobot.io/x/gobot/platforms/raspi" +) + +func main() { + r := raspi.NewAdaptor() + stepper := gpio.NewStepperDriver(r, [4]string{"7", "11", "13", "15"}, gpio.StepperModes.DualPhaseStepping, 2048) + + work := func() { + //set spped + stepper.SetSpeed(15) + + //Move forward one revolution + if err := stepper.Move(2048); err != nil { + fmt.Println(err) + } + + //Move backward one revolution + if err := stepper.Move(-2048); err != nil { + fmt.Println(err) + } + } + + robot := gobot.NewRobot("stepperBot", + []gobot.Connection{r}, + []gobot.Device{stepper}, + work, + ) + + robot.Start() +}