oap-motor.c

这个开发机器人项目源码

/*
 * $Id: oap-motor.c,v 1.9 2005/02/19 14:39:09 dwalters Exp $
 * 
 * Copyright (C) 2003 Dafydd Walters <[e-mail]>
 * 
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
#include <stdlib.h>
#include <stdio.h>
#include <unistd.h>
#include <linux/i2c.h>
#include <linux/i2c-dev.h>
#include "../../config_api/config_api.h"
#include "../include26/i2c-dev-26.h"

#define VERSION "1.0"
#define CONFIG_FILENAME "/etc/oap.conf"
#define READ_ODOMETRY_COUNTERS 20                       // SMBus Block Read
#define READ_POWER_STATE 21                             // SMBus Read Byte
#define READ_PWM_VALUES 22                              // SMBus Read Word
#define READ_TIMER 23                                   // SMBus Read Byte
#define READ_MASK 24                                    // SMBus Read Byte
#define WRITE_PWM_VALUES 120                            // SMBus Write Word
#define PWM_ON 121                                      // SMBus Send Byte
#define PWM_OFF 122                                     // SMBus Send Byte
#define ZERO_TIMER 123                                  // SMBus Send Byte
#define WRITE_MASK 124                                  // SMBus Write Byte

static int device_address;
static char filename[100];
static int file;

static void read_config();
static int check_params(int argc, char* argv[]);
static void read_odometry(int file);
static void pwm_on(int file);
static void pwm_off(int file);
static void zero_timer(int file);
static void set_pwm(int file, int left, int right);
static void set_encoder_mask(int file, int mask);
static void show_usage(void);

int main(int argc, char* argv[])
{

    if (!check_params(argc, argv)) {
        show_usage();
        exit(EXIT_FAILURE);
    }

    /* Load configuration settings */
    read_config();

    /* Open device */    
    if ((file = open(filename, O_RDWR)) < 0) {
        fprintf(stderr, "Unable to open device %s\n", filename);
        exit(EXIT_FAILURE);
    }
    
    /* Select slave device address */
    if (ioctl(file, I2C_SLAVE, device_address) < 0) {
        fprintf(stderr, "Unable to set slave device address to %d\n", 
                device_address);
        close(file);
        exit(EXIT_FAILURE);
    }

    if (strcmp(argv[1], "r") == 0) {
        /* Report odometry counter values */
        read_odometry(file);
    }
    else if (strcmp(argv[1], "s") == 0) {
        /* Set PWM values */
        set_pwm(file, atoi(argv[2]), atoi(argv[3]));
    }
    else if (strcmp(argv[1], "n") == 0) {
        /* PWM output ON */
        pwm_on(file);
    }
    else if (strcmp(argv[1], "f") == 0) {
        /* PWM output OFF */
        pwm_off(file);
    }
    else if (strcmp(argv[1], "z") == 0) {
        /* Zero timer */
        zero_timer(file);
    }
    else if (strcmp(argv[1], "e") == 0) {
        /* Set Encoder Mask Byte */
        set_encoder_mask(file, atoi(argv[2]));
    }
    
    close(file);
    exit(EXIT_SUCCESS);
}

void show_usage(void)
{
    printf("\nOpen Automaton Project Motor Control Module utility - "
           "(oap-motor) version %s\nCopyright (C) 2003 Dafydd Walters\n\n"
           "oap-motor comes with ABSOLUTYELY NO WARRANTY; for details "
           "see the\nGNU General Public License at http://www.gnu.org"
           "/licenses/gpl.html\n\n", VERSION);

    printf("Usage: oap-motor <command>\n"
            "Commands:\n"
            "  r              Report odometry counters, PWM duty cycles, state & config.\n"
            "  s <lll> <rrr>  Set Left and Right PWM duty cycle drive values\n"
            "                  (0=full reverse, 128=stop, 255=full forward)\n"
            "  n              Set output ON\n"
            "  f              Set output OFF (low power mode)\n"
            "  z              Zero timer counter\n"
            "  e <nnn>        Set encoder XOR mask byte (only bits 5 and 6 significant -\n"
            "                  bit 5 = reverse right counts, bit 6 = reverse left counts)\n"
            "\n");
}

void set_pwm(int file, int left, int right)
{
    __s32 result;
    __u16 drive;

    drive = right * 256 + left;

    /* Set PWM drive values */
    result = oap_i2c_smbus_write_word_data(file, WRITE_PWM_VALUES, drive);
    if (result < 0) {
        fprintf(stderr, "Error setting PWM drive values\n");
        close(file);
        exit(EXIT_FAILURE);
    }
}

void set_encoder_mask(int file, int mask)
{
    __s32 result;

    /* Set Encoder Mask Byte */
    result = oap_i2c_smbus_write_byte_data(file, WRITE_MASK, mask);
    if (result < 0) {
        fprintf(stderr, "Error setting encoder mask byte\n");
        close(file);
        exit(EXIT_FAILURE);
    }
}

void read_odometry(int file)
{
    __s32 result;
    __u8 values[32];
    int left, right;
    int timer;

    /* Read odometry counter values */
    result = oap_i2c_smbus_read_block_data(file, READ_ODOMETRY_COUNTERS, values);
    if (result != 5) {
        fprintf(stderr, "Error reading odometry counters\n");
        close(file);
        exit(EXIT_FAILURE);
    }

    left = values[0] + (256 * values[1]);
    right = values[2] + (256 * values[3]);
    timer = values[4];

    printf("ODOMETER COUNTER STATUS\n");
    printf(" Left count : %d\n", left);
    printf(" Right count: %d\n", right);
    printf(" Timer count: %d\n", timer);

    result = oap_i2c_smbus_read_word_data(file, READ_PWM_VALUES);
    if (result < 0) {
        fprintf(stderr, "Error reading PWM duty cycles\n");
        close(file);
        exit(EXIT_FAILURE);
    }

    left = result % 256;
    right = result / 256;

    printf("Left PWM duty cycle: %d (%d%%)\n", left, left * 100 / 256);
    printf("Right PWM duty cycle: %d (%d%%)\n", right, right * 100 / 256);
 
    result = oap_i2c_smbus_read_byte_data(file, READ_TIMER);
    if (result < 0) {
        fprintf(stderr, "Error reading timer count\n");
        close(file);
        exit(EXIT_FAILURE);
    }

    printf("Timer count: %d\n", result);

    /* Query power state */
    result = oap_i2c_smbus_read_byte_data(file, READ_POWER_STATE);
    if (result < 0) {
        fprintf(stderr, "Error reading power state\n");
        close(file);
        exit(EXIT_FAILURE);
    }

    printf("PWM power state: %s\n", result ? "Active" : "Low Power");

    /* Query encoder mask byte setting */
    result = oap_i2c_smbus_read_byte_data(file, READ_MASK);
    if (result < 0) {
        fprintf(stderr, "Error reading encoder mask\n");
        close(file);
        exit(EXIT_FAILURE);
    }
    
    printf("Encoder XOR mask byte setting: %d\n", result);    
}

void zero_timer(int file)
{
    __s32 result;

    /* Zero timer */
    result = oap_i2c_smbus_write_byte(file, ZERO_TIMER);
    if (result < 0) {
        fprintf(stderr, "Zero timer\n");
        close(file);
        exit(EXIT_FAILURE);
    }
}

void pwm_on(int file)
{
    __s32 result;

    /* Set PWM output ON */
    result = oap_i2c_smbus_write_byte(file, PWM_ON);
    if (result < 0) {
        fprintf(stderr, "Error setting PWM output ON\n");
        close(file);
        exit(EXIT_FAILURE);
    }
}

void pwm_off(int file)
{
    __s32 result;

    /* Set PWM output OFF */
    result = oap_i2c_smbus_write_byte(file, PWM_OFF);
    if (result < 0) {
        fprintf(stderr, "Error setting PWM output OFF\n");
        close(file);
        exit(EXIT_FAILURE);
    }
}

int check_params(int argc, char* argv[])
{
    if ((argc == 2) && (strcmp(argv[1], "r") == 0)) 
        return 1;
    
    if ((argc == 4) && (strcmp(argv[1], "s") == 0)) 
        return 1;
    
    if ((argc == 2) && (strcmp(argv[1], "n") == 0)) 
        return 1;
    
    if ((argc == 2) && (strcmp(argv[1], "f") == 0)) 
        return 1;
    
    if ((argc == 2) && (strcmp(argv[1], "z") == 0)) 
        return 1;
    
    if ((argc == 3) && (strcmp(argv[1], "e") == 0)) 
        return 1;
    
    return 0;
}

void read_config()
{
    config_t conf;
    char s_device_address[4];

    /* Open configuration file */
    if (!config_open(&conf, CONFIG_FILENAME, C_READ)) {
        fprintf(stderr, "Unable to open configuration file %s for reading\n", 
                CONFIG_FILENAME);
        exit(EXIT_FAILURE);
    }

    /* Read configuration settings */
    if (!config_read(&conf, "I2C", "DeviceName", filename, 100)) {
        fprintf(stderr, 
        "Unable to read DeviceName from [I2C] section of configuration file\n");
        config_close(&conf);
        exit(EXIT_FAILURE);
    }
    
    if (!config_read(&conf, "Motor Control Module", "I2CAddress", 
                s_device_address, 4)) {
        fprintf(stderr, 
        "Unable to read I2CAddress from [Motor Control Module] section of\n" 
        "configuration file.\n");
        config_close(&conf);
        exit(EXIT_FAILURE);
    }
    device_address = atoi(s_device_address);

    /* Close configuration file */
    config_close(&conf);
}