Go Down

Topic: NXT Motor Shield (Read 22942 times) previous topic - next topic

Lauszus

Jun 26, 2011, 11:21 pm Last Edit: Jun 29, 2011, 08:27 pm by Lauszus Reason: 1
Part 1:
Hallo everybody

I recently made a NXT motor shield for my arduino. It can control two NXT motors and also read the onboard encoders. In true Arduino spirit I decided to share it with the rest of the community. But first i will talk about how everything works, and then show the finished shield including a short video demonstration.

Physically connecting the motors
The NXT cable is a RJ12-like cable. The only diference is the latch is offset to the right side. The schematic for the output port are as follows:

Pin 1 (white) and 2 (black) are pwm output signals for the motor. This should be approximatly 9 volts.
Pin 3 (red) are the ground wire needed by the encoders.
Pin 4 (green) are the 4.3 volts power signal for the encoders.
Pin 5 (yellow) and 6 (blue) are the inputs from the encoders.

For more information look in the "LEGO MINDSTORMS NXT Hardware Developer Kit" (http://mindstorms.lego.com/en-us/support/files/default.aspx).

Encoders
Encoders works by sending out a pulse every time you rotate the shaft. There are two encoders in each motor. When rotating the shaft, the pulses will look like this:

By reading the pulses and counting them, the Arduino can figure out what direction the wheel is turning and for how many degrees.
To read the signal I simple use an interrupt. The problem is that the output can be a bit noise, to smooth this out I use a schmitt trigger circuit identical to the one inside the NXT (see http://focus.ti.com/lit/ds/symlink/sn74hc14.pdf and http://mindstorms.lego.com/en-us/support/files/default.aspx "LEGO MINDSTORMS NXT Hardware Developer Kit" under "Appendix 1-LEGO MINDSTORMS NXT hardware schematic").
This will smooth out the the signal, so the Arduino can measure it precisely.
For further explanation look at: http://www.brickengineer.com/pages/2008/09/05/lego-nxt-motor-wiring/ and http://en.wikipedia.org/wiki/Rotary_encoder#Incremental_rotary_encoder.

By using one interrupt per motor, one can get 1 degrees of resolution. But if two are used 0.5 degrees can be obtained - this could for example be done with the Arduino mega.

The code for reading the encoders looks like this:
Code: [Select]
#define Tach01 2
#define Tach02 4
#define Tach03 3
#define Tach04 5

volatile int TachPos1 = 0;
volatile int TachPos2 = 0;

void setup(){
  pinMode(Tach01, INPUT);
  pinMode(Tach02, INPUT);
  pinMode(Tach03, INPUT);
  pinMode(Tach04, INPUT);

  attachInterrupt(0, Tach01encoder, CHANGE); //pin 2
  attachInterrupt(1, Tach02encoder, CHANGE); //pin 3

  Serial.begin(9600);
}

void loop(){
  Serial.print(TachPos1);Serial.print("\t");
  Serial.print(TachPos2);Serial.print("\t");
  Serial.println("");
}
void Tach01encoder(){
  if((PIND & B00010100) == 0 || (PIND & B00010100) == 20)//pin 2 == pin 4
  {
    TachPos1--;
  }
  else{
    TachPos1++;
  }
}
void Tach02encoder(){
  if((PIND & B00101000) == 0 || (PIND & B00101000) == 40)//pin 3 == pin 5
  {
    TachPos2--;
  }
  else{
    TachPos2++;
  }
}


I read directly from the port registers in the interrupt to use the smallest amount of time. For further information see http://www.arduino.cc/en/Reference/PortManipulation and http://www.arduino.cc/playground/Code/BitMath.

Motors
The motors are control with a pwm signal. By increasing the pwm signal, the speed of the motor will also increase. The motor is connected with two wires as explained earlier (the white and black wire). If the white is connected to positive and the black wire is connected to negative the motor will turn one way and if connected opposite the motor will turn the other direction. This is done with a IC called a H-bridge. I use the L293D  (http://www.st.com/internet/com/TECHNICAL_RESOURCES/TECHNICAL_LITERATURE/DATASHEET/CD00000059.pdf) which can deliver enough current for the motors. It also has internal protection diodes, which is needed for protecting your circuit from high voltage spikes from the motors.
For more information on how a H-bridge works. Look at this excellent guide: http://itp.nyu.edu/physcomp/Labs/DCMotorControl for further details.

The code for the motors could look like this:
Code: [Select]

#define logicleft1 8
#define logicleft2 10
#define pwm1 11

#define logicright1 7
#define logicright2 12
#define pwm2 6

void setup()
{
  pinMode(pwm1, OUTPUT);
  pinMode(logicleft1, OUTPUT);
  pinMode(logicleft2, OUTPUT);

  pinMode(pwm2, OUTPUT);
  pinMode(logicright1, OUTPUT);
  pinMode(logicright2, OUTPUT);
}

void loop()
{
  for(int i=0;i<255;i++)
  {
    analogWrite(pwm1, i);
    digitalWrite(logicleft1, HIGH);
    digitalWrite(logicleft2, LOW);
    analogWrite(pwm2, i);
    digitalWrite(logicright1, HIGH);
    digitalWrite(logicright2, LOW);
    delay(10);
  }
  delay(1000);
  for(int i=255;i>0;i--)
  {
    analogWrite(pwm1, i);
    digitalWrite(logicleft1, HIGH);
    digitalWrite(logicleft2, LOW);
    analogWrite(pwm2, i);
    digitalWrite(logicright1, HIGH);
    digitalWrite(logicright2, LOW);
    delay(10);
  }
  delay(1000);
}


This will just keep increasing the motor speed, and then decrease again from full speed.

The shield
The finished shield look like this:


I have uploaded the schematic and the PCB made in eagle, together with the gerber files ready for shipping at http://www.dorkbotpdx.org/wiki/pcb_order - just look in the bottom of this page.

NB: The shield is licensed under a Creative Commons Attribution Noncommercial Share-Alike license, see this link for details: http://creativecommons.org/licenses/by-nc-sa/3.0/

Features:

  • Full motor control

  • 1 degrees resolution encoders on both motors

  • Socket for IR reciever

  • Reset button mounted on top

  • Power LED

  • Status LED on pin 13



I got two spare shields, so if anyone is interested I will sell them as a kit - just send me a personal message here at the forum, and we will figure out a price.

Lauszus

#1
Jun 26, 2011, 11:23 pm Last Edit: Oct 19, 2011, 05:20 pm by Lauszus Reason: 1
Part 2:
Demonstration
The first demonstration shows the robot controlled by my Apple Remote using Ken Shirriff's excellent IR library (http://www.arcfn.com/2009/08/multi-protocol-infrared-remote-library.html):

http://www.youtube.com/watch?v=w5ztJEvImb8

The code look like this:
Code: [Select]

#include <IRremote.h>

const long iup = 0x77E1D0BE;
const long idown = 0x77E1B0BE;
const long iright = 0x77E1E0BE;
const long ileft = 0x77E110BE;
const long icenter = 0x77E1BABE;
const long imenu = 0x77E140BE;
const long iplay = 0x77E17ABE;
const long irepeat = 0xFFFFFFFF;

int RECV_PIN = 9;
IRrecv irrecv(RECV_PIN);
decode_results results;

//Right motor
#define logicright1 8
#define logicright2 10
#define pwm1 11
//Left motor
#define logicleft1 7
#define logicleft2 12
#define pwm2 6

int left = 0;
int right = 1;
int backward = 0;
int forward = 1;

int pwm;
int logic1;
int logic2;

int count = 0;

void setup(){
 irrecv.enableIRIn(); // Start the receiver

 pinMode(pwm1, OUTPUT);
 pinMode(logicleft1, OUTPUT);
 pinMode(logicleft2, OUTPUT);

 pinMode(pwm2, OUTPUT);
 pinMode(logicright1, OUTPUT);
 pinMode(logicright2, OUTPUT);
}

void loop(){
 if(irrecv.decode(&results)){
   count = 0;
   //Serial.println(results.value, HEX);
   switch(results.value){
   case iup:
     move(left,forward,100);
     move(right,forward,100);
     break;
   case idown:
     move(left,backward,100);
     move(right,backward,100);
     break;
   case iright:
     move(left,forward,100);
     stop(0);
     break;
   case ileft:
     move(right,forward,100);
     stop(1);
     break;
   case icenter:
     break;
   case imenu:
     break;
   case iplay:
     break;
   case irepeat:
     break;
   }
   delay(10);
   irrecv.resume(); // Receive the next value
 }
 else{
   count++;
   delay(1);
   if(count == 100){
     stop(0);
     stop(1);
   }
 }
}
void move(int motor, int way, int power){
 if(motor == 1){//right
   pwm = pwm1;
   logic1 = logicleft1;
   logic2 = logicleft2;
 }
 if(motor == 0){//left
   pwm = pwm2;
   logic1 = logicright1;
   logic2 = logicright2;
 }
 power = map(power,0,100,0,255);
 analogWrite(pwm, power);
 if(way == 0){//backward
   digitalWrite(logic1, LOW);
   digitalWrite(logic2, HIGH);
 }
 if(way == 1){//forward
   digitalWrite(logic1, HIGH);
   digitalWrite(logic2, LOW);
 }
}
void stop(int motor){
 if(motor == 0){//left
   digitalWrite(pwm1, HIGH);
   digitalWrite(logicleft1, HIGH);
   digitalWrite(logicleft2, HIGH);
 }
 if(motor == 1){//right
   digitalWrite(pwm2, HIGH);
   digitalWrite(logicright1, HIGH);
   digitalWrite(logicright2, HIGH);
 }
}


The next example is a bit mote complicated. It is controlled by a PS3 controller connected to Processing http://processing.org/ via Bluetooth. It then sends serial data through a XBee (http://search.digikey.com/scripts/DkSearch/dksus.dll?Detail&name=XB24-ACI-001-ND) to the Arduino:

http://www.youtube.com/watch?v=7-aMCNq1Ba4

The arduino code:
Code: [Select]

//Right motor
#define logicright1 8
#define logicright2 10
#define pwm1 11
//Left motor
#define logicleft1 7
#define logicleft2 12
#define pwm2 6

int pwm;
int logic1;
int logic2;

int buffer[2];

void setup(){
 pinMode(pwm1, OUTPUT);
 pinMode(logicleft1, OUTPUT);
 pinMode(logicleft2, OUTPUT);

 pinMode(pwm2, OUTPUT);
 pinMode(logicright1, OUTPUT);
 pinMode(logicright2, OUTPUT);
 
 Serial.begin(9600);
}

void loop(){
 if(Serial.available()>1){
   for(int i=0;i<2;i++){
     buffer[i] = Serial.read()-48;
   }
   if(buffer[1] == 0){
     stop(buffer[0]);
   }
   else{
     move(buffer[0], buffer[1], 100);
   }
 }  
}

void move(int motor, int way, int power){
 if(motor == 0){//right
   pwm = pwm1;
   logic1 = logicleft1;
   logic2 = logicleft2;
 }
 if(motor == 1){//left
   pwm = pwm2;
   logic1 = logicright1;
   logic2 = logicright2;
 }
 power = map(power,0,100,0,255);
 analogWrite(pwm, power);
 if(way == 1){//backward
   digitalWrite(logic1, LOW);
   digitalWrite(logic2, HIGH);
 }
 if(way == 2){//forward
   digitalWrite(logic1, HIGH);
   digitalWrite(logic2, LOW);
 }
}
void stop(int motor){
 if(motor == 0){//right
   digitalWrite(pwm1, HIGH);
   digitalWrite(logicleft1, HIGH);
   digitalWrite(logicleft2, HIGH);
 }
 if(motor == 1){//left
   digitalWrite(pwm2, HIGH);
   digitalWrite(logicright1, HIGH);
   digitalWrite(logicright2, HIGH);
 }
}


I have uploaded the processing code as a .zip file - see the bottom of this post.

Sources:
All information can be found in: "LEGO MINDSTORMS NXT Hardware Developer Kit" (http://mindstorms.lego.com/en-us/support/files/default.aspx)

To do:

Update
I have made a second version of the shield. Which supports the Ultrasonic sensor and have custom RJ12 female sockets. See my blog for more information: http://blog.tkjelectronics.dk/2011/10/nxt-shield-ver2/

tkj

Awesome work Kristian.
I really like your work - is it possible to buy this shield, if you don't want to manufacture it yourself?

Best Regards
Thomas Jespersen

Lauszus

Yes it is, as I wrote in the description I got two spare ones including all the parts needed. I will sell it all as a kit including a IR receiver for 40$ :)
- Lauszus

devilmaycry

I cant find any processing file or the eagel files if u can please send me the url s for both of them thanks

Lauszus

Just look at the bottom of both post :)
Anyway here are the links:
http://arduino.cc/forum/index.php?action=dlattach;topic=65022.0;attach=4101
http://arduino.cc/forum/index.php?action=dlattach;topic=65022.0;attach=4031

t_bostanjyan

Hi,

I understand that this is very old post but still want to ask my question here.

I looked at the motor shied schematic and have one open question.

TACHO signals are connected to the Schmitt trigger input via resistor and capacitor circuit.
I can understand why there are 100pF cap and 10K resistor but I don't understand why 1M resistor is used there.

Thank you very much,

MRMAINT62

Very professional work!

Has anybody thought about an i2c expander for LEGO EV3 sensor ports?

Do the I2C sensor ports on the EV3 work like on the Arduino?

If so, an expansion port should be really simple. Solder ten female connectors onto a circuit board with all the wires in parallel. Plug a wire into one of them and connect it to the EV3 brick and plug in a bunch of sensors.

Modifying the software to be able to work with more port numbers is likely the harder part.

Has anyone heard of anybody attempting this?   
Life is short, Eat dessert First!
Build Robots at all other times.
Boards: UNOv3, Mega 2560, Mega ADK, Bugduino, Pro Mini 328 5v, LinkIt ONE, MKR1000, NXT, Printrbot Simple, MakeBlock laser engraver, Silhouette, Cricket Expression, Mill, Metal Lathe.

Go Up