gobot.io/x/gobot/v2@v2.1.0/examples/raspi_adafruit_servo.go (about)

     1  //go:build example
     2  // +build example
     3  
     4  //
     5  // Do not build by default.
     6  
     7  package main
     8  
     9  import (
    10  	"log"
    11  	"time"
    12  
    13  	"gobot.io/x/gobot/v2"
    14  	"gobot.io/x/gobot/v2/drivers/i2c"
    15  	"gobot.io/x/gobot/v2/platforms/raspi"
    16  )
    17  
    18  var (
    19  	// Min pulse length out of 4096
    20  	servoMin = 150
    21  	// Max pulse length out of 4096
    22  	servoMax = 700
    23  	// Limiting the max this servo can rotate (in deg)
    24  	maxDegree = 180
    25  	// Number of degrees to increase per call
    26  	degIncrease = 10
    27  	yawDeg      = 90
    28  )
    29  
    30  func degree2pulse(deg int) int32 {
    31  	pulse := servoMin
    32  	pulse += ((servoMax - servoMin) / maxDegree) * deg
    33  	return int32(pulse)
    34  }
    35  
    36  func adafruitServoMotorRunner(a *i2c.AdafruitMotorHatDriver) (err error) {
    37  	log.Printf("Servo Motor Run Loop...\n")
    38  	// Changing from the default 0x40 address because this configuration involves
    39  	// a Servo HAT stacked on top of a DC/Stepper Motor HAT on top of the Pi.
    40  	stackedHatAddr := 0x41
    41  	var channel byte = 1
    42  	deg := 90
    43  
    44  	// update the I2C address state
    45  	a.SetServoHatAddress(stackedHatAddr)
    46  
    47  	// Do not need to set this every run loop
    48  	freq := 60.0
    49  	if err = a.SetServoMotorFreq(freq); err != nil {
    50  		log.Printf("%s", err.Error())
    51  		return
    52  	}
    53  	// start in the middle of the 180-deg range
    54  	pulse := degree2pulse(deg)
    55  	if err = a.SetServoMotorPulse(channel, 0, pulse); err != nil {
    56  		log.Printf(err.Error())
    57  		return
    58  	}
    59  	// INCR
    60  	pulse = degree2pulse(deg + degIncrease)
    61  	if err = a.SetServoMotorPulse(channel, 0, pulse); err != nil {
    62  		log.Printf(err.Error())
    63  		return
    64  	}
    65  	time.Sleep(2000 * time.Millisecond)
    66  	// DECR
    67  	pulse = degree2pulse(deg - degIncrease)
    68  	if err = a.SetServoMotorPulse(channel, 0, pulse); err != nil {
    69  		log.Printf(err.Error())
    70  		return
    71  	}
    72  	return
    73  }
    74  
    75  func main() {
    76  	r := raspi.NewAdaptor()
    77  	adaFruit := i2c.NewAdafruitMotorHatDriver(r)
    78  
    79  	work := func() {
    80  		gobot.Every(5*time.Second, func() {
    81  			adafruitServoMotorRunner(adaFruit)
    82  		})
    83  	}
    84  
    85  	robot := gobot.NewRobot("adaFruitBot",
    86  		[]gobot.Connection{r},
    87  		[]gobot.Device{adaFruit},
    88  		work,
    89  	)
    90  
    91  	robot.Start()
    92  }