heuzef
/
WiringPi
mirror of https://github.com/WiringPi/WiringPi.git
1
0
Fork 0
Code Issues 0 Releases 9 Wiki Activity
Browse Source

Servo stuff

pull/158/head
Jim Parziale 2 years ago
parent
1df95d204a
commit
8d9280942c
6 changed files with 353 additions and 84 deletions
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. +0
    -19
      examples/servo-reset.sh
  2. +138
    -0
      examples/servo-test.sh
  3. +137
    -64
      examples/servo.c
  4. +46
    -0
      examples/servomotor.py
  5. +30
    -1
      wiringPi/wiringPi.c
  6. +2
    -0
      wiringPi/wiringPi.h

+ 0
- 19
examples/servo-reset.sh View File

@@ -1,19 +0,0 @@
gpio mode 1 pwm
gpio pwm-ms
gpio pwmc 192
gpio pwmr 2000
sleep 1

# 100%
gpio pwm 1 250
sleep 1

# 0%
gpio pwm 1 50
sleep 1

gpio mode 1 pwm
gpio pwm-ms
gpio pwmc 192
gpio pwmr 2000


+ 138
- 0
examples/servo-test.sh View File

@@ -0,0 +1,138 @@
pin=12

#pwmc=192
#pwmr=2000
#max=230
#mid=120
#min=20

#pwmc=512
#pwmr=256
#max=90
#mid=46
#min=20

#pwmc=4000
#pwmr=240
#max=30
#mid=16
#min=5

#pwmc=400
#pwmr=240
#max=115
#mid=60
#min=26

#pwmc=50
#pwmr=4000
#max=900
#mid=500
#min=200

#pwmc=100
#pwmr=10000
#max=450
#mid=250
#min=90
#step=5

#pwmc=50
#pwmr=10000
#max=880
#mid=500
#min=200
#step=10

pwmc=60
pwmr=10000
max=733
mid=416
min=167
step=10

(( nsteps = ($max - $min) / $step ))

#nsteps=32
#(( step = ($max - $min) / $nsteps ))
#if [[ $step -lt 1 ]]; then step=1; fi

echo "pwmc = $pwmc"
echo "pwmr = $pwmr"
echo "max = $max"
echo "min = $min"
echo "mid = $mid"
echo "nsteps = $nsteps"
echo "step = $step"

function init() {
echo "Init 1"
gpio mode $pin pwm
gpio pwm-ms

gpio pwmc 192
gpio pwmr 2000
gpio pwm $pin 120
sleep 1

echo "Init 2"
gpio pwmc $pwmc
gpio pwmr $pwmr
gpio pwm $pin $mid
sleep 1
}

function reset() {
echo "Reset"
gpio mode $pin pwm
gpio pwm-ms
gpio pwmc $pwmc
gpio pwmr $pwmr
gpio pwm $pin $mid
sleep 0.5
gpio mode $pin out
exit 0
}

function rangeUp() {
for (( i=$min; i <= $max; i = $i + $step )); do
echo "Set $i"
gpio pwm $pin $i
sleep 0.1
done
}

function rangeDn() {
for (( i=$max; i >= $min; i = $i - $step )); do
echo "Set $i"
gpio pwm $pin $i
sleep 0.1
done
}

trap reset INT TERM

init

rangeUp
sleep 1
rangeDn
sleep 1

# 100%
echo "Max"
gpio pwm $pin $max
sleep 3

# 0%
echo "Min"
gpio pwm $pin $min
sleep 3

# 50% (middle)
echo "Middle"
gpio pwm $pin $mid
sleep 3

echo "Done"
reset

+ 137
- 64
examples/servo.c View File

@@ -28,76 +28,138 @@
#include <errno.h>
#include <string.h>
#include <time.h>
#include <signal.h>
#include <libgen.h>
#include <sys/types.h>
#include <unistd.h>

#include <wiringPi.h>
#include <softServo.h>

// GPIO 1 (physical pin 12)
static const int cPin = 1;
// *****************************************************************************

int main()
// GPIO 12 (physical pin 32)
static const int cGpioPin = 12;

static const int cPwmFreq = 50; // Hz
static const int cPwmc=60;
static const int cPwmr=10000;

static const int cMin = 167;
static const int cMid = 416;
static const int cMax = 733;
static const int cStep = 10;

static void SignalHandler(int sig);

// *****************************************************************************

void init()
{
if (wiringPiSetup() == -1)
printf("Init\n");
pinMode(cGpioPin, PWM_OUTPUT);
pwmToneWrite(cGpioPin, cPwmFreq);
pwmSetMode(PWM_MODE_MS);

pwmSetClock(cPwmc);
pwmSetRange(cPwmr);
pwmWrite(cGpioPin, cMid);
delayMs(1000);
}

void reset()
{
printf("Reset\n");
pinMode(cGpioPin, PWM_OUTPUT);
pwmSetMode(PWM_MODE_MS);

pwmSetClock(cPwmc);
pwmSetRange(cPwmr);
pwmWrite(cGpioPin, cMid);
delayMs(500);

pinMode(cGpioPin, OUTPUT);
delayMs(1000);
}

void rangeUp()
{
int pos = 0;
for (int i=cMin; i <= cMax; i +=cStep )
{
fprintf(stdout, "oops: %s\n", strerror(errno));
return 1;
printf(" %d: ", i);
pwmWrite(cGpioPin, i);
delayMs(100);
pos = pwmRead(cGpioPin);
printf("%d\n", pos);
}
}

void rangeDn()
{
int pos = 0;
for (int i=cMax; i >= cMin; i -=cStep )
{
printf(" %d: ", i);
pwmWrite(cGpioPin, i);
delayMs(100);
pos = pwmRead(cGpioPin);
printf("%d\n", pos);
}
}

softServoSetup(cPin, -1, -1, -1, -1, -1, -1, -1);
// *****************************************************************************

/*
softServoWrite(1, -250);
delay(1000);
softServoWrite(1, 0);
delay(1000);
softServoWrite(1, 50);
delay(1000);
softServoWrite(1, 150);
delay(1000);
softServoWrite(1, 200);
delay(1000);
softServoWrite(1, 250);
delay(1000);
softServoWrite(1, 1250);
delay(1000);
*/

/*
softServoWrite (1, 1000) ; delay(1000);
softServoWrite (2, 1100) ; delay(1000);
softServoWrite (3, 1200) ; delay(1000);
softServoWrite (4, 1300) ; delay(1000);
softServoWrite (5, 1400) ; delay(1000);
softServoWrite (6, 1500) ; delay(1000);
softServoWrite (7, 2200) ; delay(1000);
*/

int i = 1;
int step = 0;
const int cMin = -250;
const int cMax = 1250;
const int cStep = 100;

printf("\n");
printf("Home, Max, Home...\n");
softServoWrite(cPin, cMax);
softServoWrite(cPin, cMin);
softServoWrite(cPin, cMax);
delay(1000);

printf("Stepping %d steps...\n", (cMax - cMin)/cStep);
for (step = cMin; step < cMax; step += cStep, ++i)
int main(int argc, char *argv[])
{
printf("\n%s: [%s] %s @ %d\n\n", basename(argv[0]), __FILE__, __FUNCTION__, __LINE__);

signal(SIGABRT, SignalHandler);
signal(SIGTERM, SignalHandler);
signal(SIGINT, SignalHandler);

if (wiringPiSetupGpio() == -1)
{
printf("%d...", i); fflush(stdout);
softServoWrite(cPin, step);
delay(300);
fprintf(stdout, "oops: %s\n", strerror(errno));
return 1;
}
printf("\n");
printf("Returning to home position.\n");
softServoWrite(cPin, cMin);
delay(1000);

const int cStep1 = 50;
init();

printf("Setting to middle position.\n");
pwmWrite(cGpioPin, cMid);
delayMs(1000);

printf("Moving up\n");
rangeUp();
delayMs(1000);

printf("Moving down\n");
rangeDn();
delayMs(1000);


printf("Max\n");
pwmWrite(cGpioPin, cMax);
delayMs(1000);
printf("Min\n");
pwmWrite(cGpioPin, cMin);
delayMs(1000);
printf("Max\n");
pwmWrite(cGpioPin, cMax);
delayMs(1000);
printf("Min\n");
pwmWrite(cGpioPin, cMin);
delayMs(1000);

printf("Returning to middle position.\n");
pwmWrite(cGpioPin, cMid);
delayMs(1000);

#if 0
delayMs(1000);

const int cStep1 = 5;
const int cRange = (cMax - cMin) / cStep1;
time_t t;
/* Intializes random number generator */
@@ -106,22 +168,33 @@ int main()
int pos = cMin;
int lastPos = pos;
printf("Setting %d random positions...\n", cPositions);
for (i = 1; i <= cPositions; ++i)
for (int i = 1; i <= cPositions; ++i)
{
while (pos == lastPos)
{
pos = ((rand() % cRange) * cStep1) + cMin;
}
printf("%d...", i); fflush(stdout);
softServoWrite(cPin, pos);
pwmWrite(cGpioPin, pos);
lastPos = pos;
delay(200);
delayMs(300);
}

printf("\n");
printf("Returning to home position.\n");
softServoWrite(cPin, cMin);
delay(1000);
printf("Returning to middle position.\n");
pwmWrite(cGpioPin, cMid);
delayMs(1000);
#endif

return 0;
}

// *****************************************************************************

void SignalHandler(int sig)
{
fprintf(stderr, "\nSignal %d caught by %d\n", sig, getpid());
reset();
exit(2);
}

// *****************************************************************************

+ 46
- 0
examples/servomotor.py View File

@@ -0,0 +1,46 @@
#! /usr/bin/env python3

import RPi.GPIO as GPIO
import time

servoPIN = 12
GPIO.setmode(GPIO.BCM)
GPIO.setup(servoPIN, GPIO.OUT)

print("Start");
p = GPIO.PWM(servoPIN, 50) # GPIO 12 for PWM with 50Hz

dutyMin=2.1
dutyMid=6.0
dutyMax=10.9

dutyStep=-0.1
dutySet=dutyMid

print("Center");
p.start(dutyMid)
time.sleep(3.0)

try:

while True:

print("Duty cycle " + str(dutySet));
p.ChangeDutyCycle(dutySet)
time.sleep(0.1)

dutySet += dutyStep

if dutySet > dutyMax or dutySet < dutyMin:
dutyStep = -dutyStep
dutySet += dutyStep
dutySet += dutyStep
print("");
time.sleep(1)

except KeyboardInterrupt:
print("\nCenter");
p.ChangeDutyCycle(dutyMid)
time.sleep(1.0)
p.stop()
GPIO.cleanup()

+ 30
- 1
wiringPi/wiringPi.c View File

@@ -1263,7 +1263,8 @@ static void pullUpDnControlDummy (UNUSED struct wiringPiNodeStruct *
static int digitalReadDummy (UNUSED struct wiringPiNodeStruct *node, UNUSED int UNUSED pin) { return LOW; }
static void digitalWriteDummy (UNUSED struct wiringPiNodeStruct *node, UNUSED int pin, UNUSED int value) { return; }
static void pwmWriteDummy (UNUSED struct wiringPiNodeStruct *node, UNUSED int pin, UNUSED int value) { return; }
static int analogReadDummy (UNUSED struct wiringPiNodeStruct *node, UNUSED int pin) { return 0; }
static int pwmReadDummy (UNUSED struct wiringPiNodeStruct *node, UNUSED int pin) { return 0; }
static int analogReadDummy (UNUSED struct wiringPiNodeStruct *node, UNUSED int pin) { return 0; }
static void analogWriteDummy (UNUSED struct wiringPiNodeStruct *node, UNUSED int pin, UNUSED int value) { return; }

struct wiringPiNodeStruct *wiringPiNewNode (int pinBase, int numPins)
@@ -1606,6 +1607,34 @@ void pwmWrite (int pin, int value)
}
}

int pwmRead (int pin)
{
int ret = 0;

struct wiringPiNodeStruct *node = wiringPiNodes;

setupCheck ("pwmRead");

if ((pin & PI_GPIO_MASK) == 0) // On-Board Pin
{
if (wiringPiMode == WPI_MODE_PINS)
pin = pinToGpio[pin];
else if (wiringPiMode == WPI_MODE_PHYS)
pin = physToGpio[pin];
else if (wiringPiMode != WPI_MODE_GPIO)
return 0;

usingGpioMemCheck ("pwmRead");
ret = *(pwm + gpioToPwmPort[pin]);
}
else
{
if ((node = wiringPiFindNode (pin)) != NULL)
ret = node->pwmRead (node, pin);
}
return ret;
}


/*
* analogRead:


+ 2
- 0
wiringPi/wiringPi.h View File

@@ -179,6 +179,7 @@ struct wiringPiNodeStruct
int (*digitalRead) (struct wiringPiNodeStruct *node, int pin);
void (*digitalWrite) (struct wiringPiNodeStruct *node, int pin, int value);
void (*pwmWrite) (struct wiringPiNodeStruct *node, int pin, int value);
int (*pwmRead) (struct wiringPiNodeStruct *node, int pin);
int (*analogRead) (struct wiringPiNodeStruct *node, int pin);
void (*analogWrite) (struct wiringPiNodeStruct *node, int pin, int value);

@@ -250,6 +251,7 @@ extern void setPadDrive (int group, int value);

// PWM funcs
extern void pwmWrite (int pin, int value);
extern int pwmRead (int pin);
extern void pwmToneWrite (int pin, int freq);
extern void pwmSetMode (int mode); // balanced or mark/space mode
extern void pwmSetRange (unsigned int range);


Write Preview
Loading…
Cancel
Save