Linefollower Robot

c-implementation-i2c.c

  /* ======================================================================= *//**
 * @Component			OMAPCONF
 * @Filename			i2cget.c
 * @Description			A user-space program to read an I2C register.
 * @Copyright			GPL
																			              *//*======================================================================== */
/*
Small example using parts of the below mentioned sources


    i2cget.c - A user-space program to read an I2C register.
    Copyright (C) 2005-2010  Jean Delvare <[email protected]>

    Based on i2cset.c:
    Copyright (C) 2001-2003  Frodo Looijaard <[email protected]>, and
                             Mark D. Studebaker <[email protected]>
    Copyright (C) 2004-2005  Jean Delvare <[email protected]>

    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., 51 Franklin Street, Fifth Floor, Boston,
    MA 02110-1301 USA.
*/

#include <errno.h>
#include <string.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include "omapconf/i2c-tools/i2c-dev.h"

#include <sys/types.h>
#include <sys/stat.h>
#include <sys/param.h>		/* for NAME_MAX */
#include <string.h>
#include <strings.h>		/* for strcasecmp() */
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <limits.h>
#include <dirent.h>
#include <fcntl.h>
#include <errno.h>

//#include <i2cbusses.h>
//#include <util.h>
//#include <version.h>

int
open_i2c_dev (int i2cbus, char *filename, size_t size, int quiet)
{
  int file;

  snprintf (filename, size, "/dev/i2c/%d", i2cbus);
  filename[size - 1] = '\0';
  file = open (filename, O_RDWR);

  if (file < 0 && (errno == ENOENT || errno == ENOTDIR))
    {
      sprintf (filename, "/dev/i2c-%d", i2cbus);
      file = open (filename, O_RDWR);
    }

  if (file < 0 && !quiet)
    {
      if (errno == ENOENT)
	{
	  fprintf (stderr, "Error: Could not open file "
		   "`/dev/i2c-%d' or `/dev/i2c/%d': %s\n",
		   i2cbus, i2cbus, strerror (ENOENT));
	}
      else
	{
	  fprintf (stderr, "Error: Could not open file "
		   "`%s': %s\n", filename, strerror (errno));
	  if (errno == EACCES)
	    fprintf (stderr, "Run as root?\n");
	}
    }

  return file;
}

int
set_slave_addr (int file, int address, int force)
{
  /* With force, let the user read from/write to the registers
     even when a driver is also running */
  if (ioctl (file, force ? I2C_SLAVE_FORCE : I2C_SLAVE, address) < 0)
    {
      fprintf (stderr,
	       "Error: Could not set address to 0x%02x: %s\n",
	       address, strerror (errno));
      return -errno;
    }

  return 0;
}

int
ppma (int speed)
{
  return 1500 + speed;
}


void
setM (int file, int id, int speed)
{
//  usleep (200);
  int ret = -1;
  while (1)
    {
      printf ("Setting motor %d to %d\n", id, speed);
      ret = i2c_smbus_write_word_data (file, id, speed);
      if (ret == 0)
	return;
      usleep (50);
    }
}

int
main (int argc, char *argv[])
{
  char *end;
  int res, i2cbus, address, size, file;
  int daddress;
  char filename[20];
  int pec = 0;
  int flags = 0;
  int force = 0, yes = 0, version = 0;


//I2C bus 1 filename /dev/i2c-1 address 41 Daddress 2 size 2 pec 0



  i2cbus = 1;			//lookup_i2c_bus("1");
  if (i2cbus < 0)
    return -1;

  address = 41;			// parse_i2c_address("0x29");
  if (address < 0)
    return -1;

  daddress = 2;

  /* 'w': */ size = I2C_SMBUS_WORD_DATA;


  file = open_i2c_dev (i2cbus, filename, sizeof (filename), 0);
  if (file < 0
      //|| check_funcs(file, size, daddress, pec)
      || set_slave_addr (file, address, force))
    exit (1);


  printf ("I2C bus %d filename %s address %d Daddress %d size %d pec %d\n",
	  i2cbus, filename, address, daddress, size, pec);

//      if (!yes && !confirm(filename, address, size, daddress, pec))
//              exit(0);

  if (pec && ioctl (file, I2C_PEC, 1) < 0)
    {
      fprintf (stderr, "Error: Could not set PEC: %s\n", strerror (errno));
      close (file);
      exit (1);
    }




/*


daddress=0x4;
int value = 1600;
res = i2c_smbus_write_word_data(file, daddress, value);

for(int i =0; i < 1000; i++){
	daddress =0;
 	res = i2c_smbus_read_word_data(file, daddress);
	printf("Left 0x%0*x ", size == I2C_SMBUS_WORD_DATA ? 4 : 2, res);

	daddress =2;
 	res = i2c_smbus_read_word_data(file, daddress);
	printf("Right 0x%0*x\n", size == I2C_SMBUS_WORD_DATA ? 4 : 2, res);

}


daddress=0x4;
value = 1500;
res = i2c_smbus_write_word_data(file, daddress, value);
*/


  while (1)
    {

      int sr = i2c_smbus_read_word_data (file, 2);
      int sl = i2c_smbus_read_word_data (file, 0);

      usleep (100);

      if (sr == -1 || sl == -1)
	continue;

//max is 500
      int tv = 400;
      int fs = 20;
      int wl = 0x04;
      int wr = 0x03;
      int fr = 30;
      int br = 5;


//      printf ("sr %d, sl %d\n", sr, sl);

      if (sr < tv && sl < tv)
	{
	  printf ("mode: both off line\n");
//                      servoRight.setServo(-fs);
	  setM (file, wr, ppma (-fs));
	  //      servoLeft.setServo(fs);
	  setM (file, wl, ppma (fs));
	}
      else
	{
//        usleep (100);
	  if ((sr < tv && sl > tv))
	    {
	      printf ("mode: left sensor at line\n");
	      //servoRight.setServo(-(fr));
	      setM (file, wr, ppma (-fr));
	      //servoLeft.setServo(br);
	      setM (file, wl, ppma (br));
	    }
//        else
	  {
//            usleep (100);
	    if ((sr > tv && sl < tv))
	      {
		printf ("mode: right sensor at line\n");
//                      servoRight.setServo(-(br));
		setM (file, wr, ppma (-br));
//                      servoLeft.setServo(fr);
		setM (file, wl, ppma (fr));
	      }
	  }
	}


    }



  close (file);

  if (res < 0)
    {
      fprintf (stderr, "Error: Read failed\n");
      exit (2);
    }


  exit (0);
}


/**
 * Function: i2cget
 * Role: read given data from given address of given device of given I2C bus
 * Parameters:
 *	i2cbus: I2C bus number
 *	address: I2C device address
 *	daddress: I2C device register address
 *	data: I2C device register content (returned)
 * Return:
 *	0 in case of success
 *	-1 in case of incorrect argument
 *	-8 in case of i2c dev cannot be opened
 *	-4 in case of I2C read error
 */
int
i2cget (unsigned int i2cbus, unsigned int address, unsigned int daddress,
	unsigned int *data)
{
  int res, file;
  char filename[20];

  *data = 0;
  if (i2cbus > 0xFF)
    {
      printf ("Error: I2C bus out of range (0-255)!\n");
      return -1;
    }

  if (address > 0xFF)
    {
      printf ("Error: Chip address invalid!\n");
      return -1;
    }

  if (daddress > 0xFF)
    {
      printf ("Error: Data address invalid!\n");
      return -1;
    }

  file = open_i2c_dev (i2cbus, filename, sizeof (filename), 1);
  if (file < 0
      //|| check_funcs(file, I2C_SMBUS_BYTE_DATA, daddress, 0)
      || set_slave_addr (file, address, 1))
    return -8;

  res = i2c_smbus_read_byte_data (file, daddress);
  close (file);
  if (res < 0)
    {
      fprintf (stderr, "Error: I2C Read failed\n");
      return -4;
    }
  *data = res;

  return 0;
}


/**
 * Function: i2cget_word
 * Role: read given data from given address of given device of given I2C bus
 * Parameters:
 *	i2cbus: I2C bus number
 *	address: I2C device address
 *	daddress: I2C device register address
 *	data: I2C device register content (returned)
 * Return:
 *	0 in case of success
 *	-1 in case of incorrect argument
 *	-8 in case of i2c dev cannot be opened
 *	-4 in case of I2C read error
 */
int
i2cget_word (unsigned int i2cbus, unsigned int address, unsigned int daddress,
	     unsigned int *data)
{
  int res, file;
  char filename[20];

  *data = 0;
  if (i2cbus > 0xFF)
    {
      printf ("Error: I2C bus out of range (0-255)!\n");
      return -1;
    }

  if (address > 0xFF)
    {
      printf ("Error: Chip address invalid!\n");
      return -1;
    }

  if (daddress > 0xFF)
    {
      printf ("Error: Data address invalid!\n");
      return -1;
    }

  file = open_i2c_dev (i2cbus, filename, sizeof (filename), 1);
  if (file < 0
      //|| check_funcs(file, I2C_SMBUS_WORD_DATA, daddress, 0)
      || set_slave_addr (file, address, 1))
    return -8;

  res = i2c_smbus_read_word_data (file, daddress);
  close (file);
  if (res < 0)
    {
      fprintf (stderr, "Error: I2C Read failed\n");
      return -4;
    }
  *data = res;

  return 0;
}