Motors and Encoders

The following programs show how to control the motors in closed loop, as well as how to read the encoders.

The motors and encoders can be accessed using the device file system. Each motor is mapped to an specific direction at

/sys/class/tacho-motor/motor#/

were # can be 0 to 3 that correspond to the different motors. The actual port name (A to D) can be determined by running the following shell script command:


for f in /sys/class/tacho-motor/*; do echo -n "$f: "; cat $f/address; done

Notice that the port names and the motor number may have different order. The following is a typical result for a brick connected to three motors:


/sys/class/tacho-motor/motor0: outA
/sys/class/tacho-motor/motor1: outB
/sys/class/tacho-motor/motor2: outC

Closed Loop Control


/*
* Robot Navigation Program
* www.robotnav.com
*
* (C) Copyright 2017 Lauro Ojeda
*
* 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 <fstream>
#include <iostream>

const int SPEED = 36; //[count/s] LEGO has 360 cnt/rev
char MOTOR_SPEED_SP[] = "/sys/class/tacho-motor/motor0/speed_sp";
char MOTOR_COMMAND[] = "/sys/class/tacho-motor/motor0/command";
using namespace std;

int main()
{
ofstream speed_sp;
ofstream command;
speed_sp.open(MOTOR_SPEED_SP);
if(!speed_sp.is_open())
{
cout << "Failed to open file speed_sp";
return -1; //Failed to open device
}
command.open(MOTOR_COMMAND);
if(!command.is_open())
{
cout << "Failed to open file speed_sp";
return -1; //Failed to open device
}

cout << "Ready!";
speed_sp << SPEED << endl;
command << "run-forever" << endl;
sleep(5);
cout << "Increase speed!";
speed_sp << SPEED*4 << endl;
command << "run-forever" << endl;
sleep(5);
command << "stop" << endl;

command.close();
speed_sp.close();
return 0;
}

 

Reading Encoders


/*
* Robot Navigation Program
* www.robotnav.com
*
* (C) Copyright 2017 Lauro Ojeda
*
* 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 <fstream>
#include <iostream>

char MOTOR_ENCODER[] = "/sys/class/tacho-motor/motor0/position";
using namespace std;

int main()
{
string encoder;
ofstream position_file_out;
ifstream position_file_in;
position_file_out.open(MOTOR_ENCODER);
// Open file to reset encoders
if(!position_file_out.is_open())
{
cout << "Failed to open file position";
return -1; //Failed to open device
}
position_file_out << "0" << endl;
position_file_out.close();
while(1)
{
position_file_in.open(MOTOR_ENCODER);
position_file_in >> encoder;
cout << encoder << endl;
position_file_in.close();
}
return 0;
}