Encoders

The motor encoders can be read as any other EV3 sensor. The following code shows how to setup and access the encoder readings. The motor is controlled in closed loop. One example operates on a single motor and the other on two motors:


Single Motor Control

/*
 * Robot Navigation Program
 * www.robotnav.com
 *
 * (C) Copyright 2013 - 2014 Navigation Solutions, LLC
 *
 * 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 3 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, see <http://www.gnu.org/licenses/>.
 */
#include <fcntl.h>
#include <stdio.h>
#include  <sys/mman.h>
#include "lms2012.h"
// Motor power 0..100
const int MOTOR_SPEED= 10;
// The motor operations use a single bit (or a combination of them) 
// to determine which motor(s) will be used
// A = 0x01, B = 0x02, C = 0x04, D = 0x08
// AC = 0x05
const char MOTOR_PORT_A = 0x01; 
const char MOTOR_PORT_D = 0x08; 
const int MAX_READINGS = 10000;
int main()
{
	MOTORDATA *pMotorData;

	char motor_command[4];
	int motor_file;
	int encoder_file;
	int i;
	//Open the device file asscoiated to the motor controlers
	if((motor_file = open(PWM_DEVICE_NAME, O_WRONLY)) == -1)
	{
		printf("Failed to open device\n");
		return -1; //Failed to open device
	}
	//Open the device file asscoiated to the motor encoders 
	if((encoder_file = open(MOTOR_DEVICE_NAME, O_RDWR | O_SYNC)) == -1)
		return -1; //Failed to open device
	pMotorData  =  (MOTORDATA*)mmap(0, sizeof(MOTORDATA)*vmOUTPUTS, PROT_READ | PROT_WRITE, MAP_FILE | MAP_SHARED, encoder_file, 0);
	if (pMotorData == MAP_FAILED)
	{
		printf("Map failed\n");
		return -1;
	}
	// All motor operations use the first command byte to indicate the type of operation
	// and the second one to indicate the motor(s) port(s)
	motor_command[0] = opOUTPUT_SPEED;
	motor_command[1] = MOTOR_PORT_A; 
	motor_command[2] = MOTOR_SPEED;
	
	motor_command[0] = opOUTPUT_SPEED;
	motor_command[1] = MOTOR_PORT_D; 
	motor_command[2] = MOTOR_SPEED*2;
	write(motor_file,motor_command,3);
	// Start the motor
	motor_command[0] = opOUTPUT_START;
	motor_command[1] = MOTOR_PORT_A | MOTOR_PORT_D; 
	write(motor_file,motor_command,2);
	// Read encoders while running the motor
	for(i=1;i<MAX_READINGS;i++)
		printf("Spd/Cnt/Snr: A=%d/%d/%d D=%d/%d/%d\n",\
				pMotorData[0].Speed,pMotorData[0].TachoCounts,pMotorData[0].TachoSensor,\
				pMotorData[3].Speed,pMotorData[3].TachoCounts,pMotorData[3].TachoSensor);
	// Stop the motor
	motor_command[0] = opOUTPUT_STOP;
	write(motor_file,motor_command,2);
	// Close device files
	close(encoder_file);
	close(motor_file);
	return 0;
}


Controlling Two Motors

#include <fcntl.h>
#include <stdio.h>
#include  <sys/mman.h>
#include "lms2012.h"
// Motor power 0..100
const int MOTOR_SPEED= 10;
// The motor operations use a single bit (or a combination of them) 
// to determine which motor(s) will be used
// A = 0x01, B = 0x02, C = 0x04, D = 0x08
// AC = 0x05
const char MOTOR_PORT_A = 0x01; 
const char MOTOR_PORT_D = 0x08; 
const int MAX_READINGS = 10000;
int main()
{
	MOTORDATA *pMotorData;

	char motor_command[4];
	int motor_file;
	int encoder_file;
	int i;
	//Open the device file asscoiated to the motor controlers
	if((motor_file = open(PWM_DEVICE_NAME, O_WRONLY)) == -1)
	{
		printf("Failed to open device\n");
		return -1; //Failed to open device
	}
	//Open the device file asscoiated to the motor encoders 
	if((encoder_file = open(MOTOR_DEVICE_NAME, O_RDWR | O_SYNC)) == -1)
		return -1; //Failed to open device
	pMotorData  =  (MOTORDATA*)mmap(0, sizeof(MOTORDATA)*vmOUTPUTS, PROT_READ | PROT_WRITE, MAP_FILE | MAP_SHARED, encoder_file, 0);
	if (pMotorData == MAP_FAILED)
	{
		printf("Map failed\n");
		return -1;
	}
	// All motor operations use the first command byte to indicate the type of operation
	// and the second one to indicate the motor(s) port(s)
	motor_command[0] = opOUTPUT_SPEED;
	motor_command[1] = MOTOR_PORT_A; 
	motor_command[2] = MOTOR_SPEED;
	
	motor_command[0] = opOUTPUT_SPEED;
	motor_command[1] = MOTOR_PORT_D; 
	motor_command[2] = MOTOR_SPEED*2;
	write(motor_file,motor_command,3);
	// Start the motor
	motor_command[0] = opOUTPUT_START;
	motor_command[1] = MOTOR_PORT_A | MOTOR_PORT_D; 
	write(motor_file,motor_command,2);
	// Read encoders while running the motor
	for(i=1;i<MAX_READINGS;i++)
		printf("Spd/Cnt/Snr: A=%d/%d/%d D=%d/%d/%d\n",\
				pMotorData[0].Speed,pMotorData[0].TachoCounts,pMotorData[0].TachoSensor,\
				pMotorData[3].Speed,pMotorData[3].TachoCounts,pMotorData[3].TachoSensor);
	// Stop the motor
	motor_command[0] = opOUTPUT_STOP;
	write(motor_file,motor_command,2);
	// Close device files
	close(encoder_file);
	close(motor_file);
	return 0;
}