MirrorSave
/
pigpio
mirror of https://github.com/joan2937/pigpio
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
pigpio/demolib.c

674 lines
15 KiB
C
Raw Blame History

/*
This is free and unencumbered software released into the public domain.
Anyone is free to copy, modify, publish, use, compile, sell, or
distribute this software, either in source code form or as a compiled
binary, for any purpose, commercial or non-commercial, and by any
means.
In jurisdictions that recognize copyright laws, the author or authors
of this software dedicate any and all copyright interest in the
software to the public domain. We make this dedication for the benefit
of the public at large and to the detriment of our heirs and
successors. We intend this dedication to be an overt act of
relinquishment in perpetuity of all present and future rights to this
software under copyright law.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
IN NO EVENT SHALL THE AUTHORS BE LIABLE FOR ANY CLAIM, DAMAGES OR
OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
OTHER DEALINGS IN THE SOFTWARE.
For more information, please refer to <http://unlicense.org/>
*/
/*
This version is for pigpio version 3+
*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <fcntl.h>
#include <linux/i2c-dev.h>
#include <sys/ioctl.h>
#include "pigpio.h"
/* ===========================================================================
THIS PROGRAM NEEDS THE I2C DEVICE AND DEVELOPMENT LIBRARY
TO GET THE NEEDED FILES DO
sudo apt-get install libi2c-dev
BEFORE RUNNING THE PROGRAM ENSURE THAT THE I2C DEVICE IS PRESENT
sudo modprobe i2c-bcm2708
sudo modprobe i2c-dev
sudo chmod o+rw /dev/i2c*
=========================================================================== */
/*
P1 Name gpio used for
3 SDA 0/2 i2c
5 SCL 1/3 i2c
7 --- 4 LASER
8 TXD 14 LED1
10 RXD 15 LED2
11 --- 17 SERVO 1
12 --- 18 SERVO 2
13 --- 21/27 SERVO 3
15 --- 22 LED3
16 --- 23 TI Launchpad
18 --- 24 Sonar trigger
19 MOSI 10 Sonar echo
21 MISO 9 Motor B In 1
22 --- 25 LDR
23 SCLK 11 Motor B In 2
24 CE0 8 Motor A In1
26 CE1 7 Motor A In2
*/
#define LASER 4
#define MOTOR_A_IN2 7
#define MOTOR_A_IN1 8
#define MOTOR_B_IN1 9
#define SONAR_ECHO 10
#define MOTOR_B_IN2 11
#define LED1 14
#define LED2 15
#define SERVO1 17
#define SERVO2 18
#define SERVO3 21
#define LED3 22
#define LAUNCHPAD 23
#define SONAR_TRIGGER 24
#define LDR 25
#define LEDS 4
short rawAcc[3];
short rawGyr[3];
short rawMag[3];
#define ROLL 0
#define PITCH 1
#define YAW 2
#define ACC_ORIENTATION(X, Y, Z) \
{rawAcc[ROLL] = -X; rawAcc[PITCH] = -Y; rawAcc[YAW] = Z;}
#define GYRO_ORIENTATION(X, Y, Z) \
{rawGyr[ROLL] = Y; rawGyr[PITCH] = -X; rawGyr[YAW] = -Z;}
#define MAG_ORIENTATION(X, Y, Z) \
{rawMag[ROLL] = X; rawMag[PITCH] = Y; rawMag[YAW] = -Z;}
#define CALIBRATIONS 200
#define ADXL345_I2C_ADDR 0x53
#define ITG3200_I2C_ADDR 0x68
static int version, micros=5, millis=100;
static volatile unsigned long launchpadPulses;
static volatile unsigned long launchpad5;
static volatile unsigned long launchpad10;
static volatile unsigned long launchpad15;
static volatile unsigned long launchpadOutRange;
static volatile int launchpadErr;
static volatile uint32_t LDRrechargeTick;
/* forward prototypes */
void LEDlaserTick (void);
void motorTick(void);
void i2cTick (void);
void servoTick (void);
void sonarLDRtick(void);
void launchpadAlert(int gpio, int level, uint32_t tick);
void sonarAlert(int gpio, int level, uint32_t tick);
void LDRalert(int gpio, int level, uint32_t tick);
void putTTY(char * buf);
void putTTYstr(int row, int col, char * buf);
int main(int argc, char *argv[])
{
char str[256];
if (argc > 1) micros = atoi(argv[1]);
if (argc > 2) millis = atoi(argv[2]);
putTTY("\033c"); /* clear console */
gpioCfgBufferSize(millis);
gpioCfgClock(micros, PI_CLOCK_PCM, PI_CLOCK_PLLD);
/* before using the library you must call gpioInitialise */
version = gpioInitialise();
if (version >= 0)
{
/* initialise pins, only gpio numbers are supported */
gpioSetMode(SERVO1, PI_OUTPUT);
gpioSetMode(SERVO2, PI_OUTPUT);
gpioSetMode(SERVO3, PI_OUTPUT);
gpioSetMode(LASER, PI_OUTPUT);
gpioSetMode(LED1, PI_OUTPUT);
gpioSetMode(LED2, PI_OUTPUT);
gpioSetMode(LED3, PI_OUTPUT);
gpioSetMode(MOTOR_A_IN1, PI_OUTPUT);
gpioSetMode(MOTOR_A_IN2, PI_OUTPUT);
gpioSetMode(MOTOR_B_IN1, PI_OUTPUT);
gpioSetMode(MOTOR_B_IN2, PI_OUTPUT);
gpioSetMode(SONAR_TRIGGER, PI_OUTPUT);
gpioWrite (SONAR_TRIGGER, PI_OFF);
gpioSetMode(SONAR_ECHO, PI_INPUT);
gpioSetMode(LAUNCHPAD, PI_INPUT);
gpioSetMode(LDR, PI_INPUT);
/* update i2c fifty times a second, timer #0 */
gpioSetTimerFunc(0, 20, i2cTick);
//gpioSetTimerFunc(0, 1000, servoTick);
/* update LEDs and laser once a second, timer #1 */
gpioSetTimerFunc(1, 1000, LEDlaserTick);
/* update motors every three seconds, timer #2 */
gpioSetTimerFunc(2, 3000, motorTick);
/* update sonar/LDR 10 times a second, timer #3 */
gpioSetTimerFunc(3, 100, sonarLDRtick);
/* an attachecd TI launchpad is transmitting high pulses of
15, 35, 55, 75, ..., 975, 995 microseconds repeating with 50
microseconds off between each pulse */
gpioSetAlertFunc(LAUNCHPAD, launchpadAlert);
/* monitor sonar echos */
gpioSetAlertFunc(SONAR_ECHO, sonarAlert);
/* monitor LDR level changes */
gpioSetAlertFunc(LDR, LDRalert);
while (1)
{
sleep(1);
sprintf(str, "TI pulses %8ld", launchpadPulses);
putTTYstr(9, 1, str);
sprintf(str, "+/-5 %8ld", launchpad5);
putTTYstr(10, 6, str);
sprintf(str, "+/-10 %8ld", launchpad10);
putTTYstr(11, 5, str);
sprintf(str, "+/-15 %8ld", launchpad15);
putTTYstr(12, 5, str);
sprintf(str, "Others %8ld (last %d) ",
launchpadOutRange, launchpadErr);
putTTYstr(13, 4, str);
}
}
gpioTerminate();
return 0;
}
void LEDlaserTick(void)
{
static int gpio[LEDS]={LED1, LED2, LED3, LASER};
static int pos [LEDS]={ 0, 3, 6, 9};
static int inc [LEDS]={ 1, 1, 1, 1};
static int vals[] = {0, 1, 2, 4, 8, 16, 32, 64, 128, 249};
int i;
for (i=0; i<LEDS; i++)
{
gpioPWM(gpio[i], vals[pos[i]]);
pos[i] += inc[i];
if ( (pos[i]>=(sizeof(vals)/4)) || (pos[i]<0) )
{
inc[i] = -inc[i];
pos[i] += inc[i];
}
}
}
void sonarLDRtick(void)
{
/* trigger a sonar reading */
gpioWrite(SONAR_TRIGGER, PI_ON);
usleep(20);
gpioWrite(SONAR_TRIGGER, PI_OFF);
/* trigger a LDR reading */
gpioSetMode(LDR, PI_OUTPUT); /* drain capacitor */
gpioWrite(LDR, PI_OFF);
usleep(200);
LDRrechargeTick = gpioTick();
gpioSetMode(LDR, PI_INPUT); /* start capacitor recharge */
}
void motorTick(void)
{
static int gpio_in1[2]={MOTOR_A_IN1, MOTOR_B_IN1};
static int gpio_in2[2]={MOTOR_A_IN2, MOTOR_B_IN2};
static int speed [2]={ 80, 80};
static int inc [2]={ -50, 50};
int i;
char str[256];
for (i=0; i<2; i++)
{
speed[i]+=inc[i];
if (speed[i]<0)
{
gpioPWM(gpio_in1[i], -speed[i]);
gpioPWM(gpio_in2[i], 0);
if (speed[i] < -205) inc[i] = -inc[i];
sprintf(str, "MOT%d IN1=%3d IN2=%3d", i+1, -speed[i], 0);
}
else
{
gpioPWM(gpio_in2[i], speed[i]);
gpioPWM(gpio_in1[i], 0);
if (speed[i] > 205) inc[i] = -inc[i];
sprintf(str, "MOT%d IN1=%3d IN2=%3d", i+1, 0, speed[i]);
}
if (i) putTTYstr(7, 1, str); else putTTYstr(5, 1, str);
}
}
/* loads of code to read/write i2c */
void selectDevice(int i2c, int addr, char * name)
{
if (ioctl(i2c, I2C_SLAVE, addr) < 0)
{
fprintf(stderr, "%s not present\n", name);
}
}
void writeToDevice(int i2c, char * buf, int len)
{
static int reported = 0;
if (write(i2c, buf, len) != len)
{
if (!reported)
{
fprintf(stderr, "Can't write to device\n");
reported = 1;
}
}
else reported = 0;
}
void readADXL345(int i2c)
{
char buf[8];
static int reported = 0;
selectDevice(i2c, ADXL345_I2C_ADDR, "ADXL345");
writeToDevice(i2c, "\x32", 1);
if (read(i2c, buf, 6) != 6)
{
if (!reported)
{
fprintf(stderr, "Unable to read from ADXL345\n");
reported = 1;
}
}
else
{
reported = 0;
ACC_ORIENTATION (
((buf[1]<<8) | buf[0]),
((buf[3]<<8) | buf[2]),
((buf[5]<<8) | buf[4]) );
}
}
void readITG3200(int i2c)
{
char buf[8];
static int reported = 0;
selectDevice(i2c, ITG3200_I2C_ADDR, "ITG3200");
writeToDevice(i2c, "\x1D", 1);
if (read(i2c, buf, 6) != 6)
{
if (!reported)
{
fprintf(stderr, "Unable to read from ITG3200\n");
reported = 1;
}
}
else
{
reported = 0;
GYRO_ORIENTATION (
((buf[0]<<8) | buf[1]),
((buf[2]<<8) | buf[3]),
((buf[4]<<8) | buf[5]) );
}
}
int initI2Cdevices(void)
{
int i2c;
if ((i2c = open("/dev/i2c-0", O_RDWR)) < 0)
{
perror("Failed to open i2c bus");
exit(1);
}
/* initialise ADXL345 */
selectDevice(i2c, ADXL345_I2C_ADDR, "ADXL345");
writeToDevice(i2c, "\x2d\x00", 2);
writeToDevice(i2c, "\x2d\x10", 2);
writeToDevice(i2c, "\x2d\x08", 2);
writeToDevice(i2c, "\x31\x00", 2);
writeToDevice(i2c, "\x31\x0b", 2);
/* initialise ITG3200 */
selectDevice(i2c, ITG3200_I2C_ADDR, "ITG3200");
writeToDevice(i2c, "\x16\b00011000", 2);
return i2c;
}
/* an attached IMU (GY-85) supplies orientation information which
is used to position the servos */
float estimateAngle(int acc, int gyro, float oldAng, int elapsed)
{
float angleAcc, angleInc, estAngle;
float secs;
secs = (float) elapsed / 1e6f;
angleAcc = (float) acc * 90.0f / 256.0f;
angleInc = (float) gyro * secs * 2000.0f / 32768.0f;
estAngle = 0.75 * (oldAng + angleInc) + 0.25 * angleAcc;
return estAngle;
}
void servoTick(void)
{
static int wid1=1500, wid2=1500, wid3=1500;
static int inc1=50, inc2=75, inc3=100;
gpioServo(SERVO1, wid1);
gpioServo(SERVO2, wid2);
gpioServo(SERVO3, wid3);
wid1+=inc1; if ((wid1<1000) || (wid1>2000)) {inc1 = -inc1; wid1+=inc1;}
wid2+=inc2; if ((wid2<1000) || (wid2>2000)) {inc2 = -inc2; wid2+=inc2;}
wid3+=inc3; if ((wid3<1000) || (wid3>2000)) {inc3 = -inc3; wid3+=inc3;}
}
void i2cTick(void)
{
static int inited = 0;
static int calibrated = 0;
static int calibrations = 0;
static int accCalibX = 0, accCalibY = 0, accCalibZ = 0;
static int gyroCalibX = 0, gyroCalibY = 0, gyroCalibZ = 0;
static int i2c;
static float X=0.0, Y=0.0, Z=0.0;
static uint32_t lastTick;
uint32_t tick;
int elapsed;
int pulse;
char str[256];
if (inited)
{
tick = gpioTick();
elapsed = tick - lastTick;
lastTick = tick;
readADXL345(i2c);
readITG3200(i2c);
if (calibrated)
{
X = estimateAngle(
rawAcc[ROLL], rawGyr[ROLL] -gyroCalibX, X, elapsed);
Y = estimateAngle(
rawAcc[PITCH], rawGyr[PITCH] - gyroCalibY, Y, elapsed);
Z = estimateAngle(
rawAcc[YAW], rawGyr[YAW] - gyroCalibZ, Z, elapsed);
pulse = 1500 + (Y * 1000 / 90);
if (pulse < 500) pulse = 500;
if (pulse > 2500) pulse = 2500;
gpioServo(SERVO1, pulse);
pulse = 1500 - (X * 500 / 90);
if (pulse < 1000) pulse = 1000;
if (pulse > 2000) pulse = 2000;
gpioServo(SERVO2, pulse);
/* prefer Z but that doesn't change much */
pulse = 1500 - (Y * 500 / 90);
if (pulse < 800) pulse = 800;
if (pulse > 2200) pulse = 2200;
gpioServo(SERVO3, pulse);
sprintf(str, "X=%4.0f Y=%4.0f Z=%4.0f ", X, Y, Z);
putTTYstr(1, 1, str);
}
else
{
accCalibX+=rawAcc[ROLL];
accCalibY+=rawAcc[PITCH];
accCalibZ+=rawAcc[YAW];
gyroCalibX+=rawGyr[ROLL];
gyroCalibY+=rawGyr[PITCH];
gyroCalibZ+=rawGyr[YAW];
if (++calibrations >= CALIBRATIONS)
{
accCalibX /= CALIBRATIONS;
accCalibY /= CALIBRATIONS;
accCalibZ /= CALIBRATIONS;
gyroCalibX /= CALIBRATIONS;
gyroCalibY /= CALIBRATIONS;
gyroCalibZ /= CALIBRATIONS;
calibrated = 1;
}
}
}
else
{
i2c = initI2Cdevices();
gpioServo(SERVO1, 1500);
gpioServo(SERVO2, 1500);
gpioServo(SERVO3, 1500);
inited = 1;
}
}
void sonarAlert(int gpio, int level, uint32_t tick)
{
static uint32_t startTick;
int diffTick;
char str[256];
if (level == PI_ON)
{
startTick = tick;
}
else if (level == PI_OFF)
{
diffTick = tick - startTick;
if (diffTick < 26100)
{
sprintf(str, "Sonar %3d cms", (diffTick+29)/58);
putTTYstr(15, 1, str);
}
}
}
void LDRalert(int pin, int level, uint32_t tick)
{
int diffTick;
char str[256];
if (level == PI_ON)
{
diffTick = tick - LDRrechargeTick;
sprintf(str, "LDR %4d micros", diffTick);
putTTYstr(17, 1, str);
}
}
void launchpadAlert(int pin, int level, uint32_t tick)
{
static int inited = 0, lastTick, lastPulseLen;
int pulseLen, pulseDif;
if (inited)
{
pulseLen = tick - lastTick;
lastTick = tick;
if (level==0)
{
if (lastPulseLen)
{
pulseDif = pulseLen - lastPulseLen;
/* allow for wrap around */
if (pulseDif < 0) pulseDif += 1096;
/* now centre around expected value */
pulseDif -= 33;
if (pulseDif < 0) pulseDif = -pulseDif;
launchpadPulses++;
if (pulseDif <= 5)
{
launchpad5++;
}
else if (pulseDif <= 10)
{
launchpad10++;
}
else if (pulseDif <= 15)
{
launchpad15++;
}
else
{
launchpadOutRange++;
launchpadErr = pulseDif;
}
}
lastPulseLen = pulseLen;
}
}
else
{
lastTick = tick;
lastPulseLen = 0;
launchpadPulses = 0;
launchpad5 = 0;
launchpad10 = 0;
launchpad15 = 0;
launchpadOutRange = 0;
inited = 1;
}
}
void putTTY(char * buf)
{
write(1, buf, strlen(buf));
}
void putTTYstr(int row, int col, char * buf)
{
char str[256];
sprintf(str, "\033[%d;%dH%s", row, col, buf);
putTTY(str);
}
Reference in New Issue View Git Blame Copy Permalink