10 files changed, 915 insertions, 0 deletions
diff --git a/libraries/Robot_Motor/ArduinoRobotMotorBoard.cpp b/libraries/Robot_Motor/ArduinoRobotMotorBoard.cpp
new file mode 100644
index 0000000..7740a06
--- /dev/null
+++ b/libraries/Robot_Motor/ArduinoRobotMotorBoard.cpp
@@ -0,0 +1,265 @@
+#include "ArduinoRobotMotorBoard.h"
+#include "EasyTransfer2.h"
+#include "Multiplexer.h"
+#include "LineFollow.h"
+
+RobotMotorBoard::RobotMotorBoard(){
+	//LineFollow::LineFollow();
+}
+/*void RobotMotorBoard::beginIRReceiver(){
+	IRrecv::enableIRIn();
+}*/
+void RobotMotorBoard::begin(){
+	//initialze communication
+	Serial1.begin(9600);
+	messageIn.begin(&Serial1);
+	messageOut.begin(&Serial1);
+
+	//init MUX
+	uint8_t MuxPins[]={MUXA,MUXB,MUXC};
+	this->IRs.begin(MuxPins,MUX_IN,3);
+	pinMode(MUXI,INPUT);
+	digitalWrite(MUXI,LOW);
+	
+	isPaused=false;
+}
+
+void RobotMotorBoard::process(){
+	if(isPaused)return;//skip process if the mode is paused
+	
+	if(mode==MODE_SIMPLE){
+		//Serial.println("s");
+		//do nothing? Simple mode is just about getting commands
+	}else if(mode==MODE_LINE_FOLLOW){
+		//do line following stuff here.
+		LineFollow::runLineFollow();
+	}else if(mode==MODE_ADJUST_MOTOR){
+		//Serial.println('a');
+		//motorAdjustment=analogRead(POT);
+		//setSpeed(255,255);
+		//delay(100);
+	}
+}
+void RobotMotorBoard::pauseMode(bool onOff){
+	if(onOff){
+		isPaused=true;
+	}else{
+		isPaused=false;
+	}
+	stopCurrentActions();
+
+}
+void RobotMotorBoard::parseCommand(){
+	uint8_t modeName;
+	uint8_t codename;
+	int value;
+	int speedL;
+	int speedR;
+	if(this->messageIn.receiveData()){
+		//Serial.println("data received");
+		uint8_t command=messageIn.readByte();
+		//Serial.println(command);
+		switch(command){
+			case COMMAND_SWITCH_MODE:
+				modeName=messageIn.readByte();
+				setMode(modeName);
+				break;
+			case COMMAND_RUN:
+				if(mode==MODE_LINE_FOLLOW)break;//in follow line mode, the motor does not follow commands
+				speedL=messageIn.readInt();
+				speedR=messageIn.readInt();
+				motorsWrite(speedL,speedR);
+				break;
+			case COMMAND_MOTORS_STOP:
+				motorsStop();
+				break;
+			case COMMAND_ANALOG_WRITE:
+				codename=messageIn.readByte();
+				value=messageIn.readInt();
+				_analogWrite(codename,value);
+				break;
+			case COMMAND_DIGITAL_WRITE:
+				codename=messageIn.readByte();
+				value=messageIn.readByte();
+				_digitalWrite(codename,value);
+				break;
+			case COMMAND_ANALOG_READ:
+				codename=messageIn.readByte();
+				_analogRead(codename);
+				break;
+			case COMMAND_DIGITAL_READ:
+				codename=messageIn.readByte();
+				_digitalRead(codename);
+				break;
+			case COMMAND_READ_IR:
+				_readIR();
+				break;
+			case COMMAND_READ_TRIM:
+				_readTrim();
+				break;
+			case COMMAND_PAUSE_MODE:
+				pauseMode(messageIn.readByte());//onOff state
+				break;
+			case COMMAND_LINE_FOLLOW_CONFIG:
+				LineFollow::config(
+					messageIn.readByte(),	//KP
+					messageIn.readByte(),	//KD
+					messageIn.readByte(),	//robotSpeed
+					messageIn.readByte()	//IntegrationTime
+				);
+				break;
+		}
+	}
+	//delay(5);
+}
+uint8_t RobotMotorBoard::parseCodename(uint8_t codename){
+	switch(codename){
+		case B_TK1:
+			return TK1;
+		case B_TK2:
+			return TK2;
+		case B_TK3:
+			return TK3;
+		case B_TK4:
+			return TK4;
+	}
+}
+uint8_t RobotMotorBoard::codenameToAPin(uint8_t codename){
+	switch(codename){
+		case B_TK1:
+			return A0;
+		case B_TK2:
+			return A1;
+		case B_TK3:
+			return A6;
+		case B_TK4:
+			return A11;
+	}
+}
+
+void RobotMotorBoard::setMode(uint8_t mode){
+	if(mode==MODE_LINE_FOLLOW){
+		LineFollow::calibIRs();
+	}
+	/*if(mode==SET_MOTOR_ADJUSTMENT){
+			save_motor_adjustment_to_EEPROM();
+		}
+	*/
+	/*if(mode==MODE_IR_CONTROL){
+		beginIRReceiver();
+	}*/
+	this->mode=mode;
+	//stopCurrentActions();//If line following, this should stop the motors
+}
+
+void RobotMotorBoard::stopCurrentActions(){
+	motorsStop();
+	//motorsWrite(0,0);
+}
+
+void RobotMotorBoard::motorsWrite(int speedL, int speedR){
+	/*Serial.print(speedL);
+	Serial.print(" ");
+	Serial.println(speedR);*/
+	//motor adjustment, using percentage
+	_refreshMotorAdjustment();
+	
+	if(motorAdjustment<0){
+		speedR*=(1+motorAdjustment);
+	}else{
+		speedL*=(1-motorAdjustment);
+	}
+	
+	if(speedL>0){
+		analogWrite(IN_A1,speedL);
+		analogWrite(IN_A2,0);
+	}else{
+		analogWrite(IN_A1,0);
+		analogWrite(IN_A2,-speedL);
+	}
+	
+	if(speedR>0){
+		analogWrite(IN_B1,speedR);
+		analogWrite(IN_B2,0);
+	}else{
+		analogWrite(IN_B1,0);
+		analogWrite(IN_B2,-speedR);
+	}
+}
+void RobotMotorBoard::motorsWritePct(int speedLpct, int speedRpct){
+	//speedLpct, speedRpct ranges from -100 to 100
+	motorsWrite(speedLpct*2.55,speedRpct*2.55);
+}
+void RobotMotorBoard::motorsStop(){
+	analogWrite(IN_A1,255);
+	analogWrite(IN_A2,255);
+
+	analogWrite(IN_B1,255);
+	analogWrite(IN_B2,255);
+}
+
+
+/*
+*
+*
+*	Input and Output ports
+*
+*
+*/
+void RobotMotorBoard::_digitalWrite(uint8_t codename,bool value){
+	uint8_t pin=parseCodename(codename);
+	digitalWrite(pin,value);
+}
+void RobotMotorBoard::_analogWrite(uint8_t codename,int value){
+	//There's no PWM available on motor board
+}
+void RobotMotorBoard::_digitalRead(uint8_t codename){
+	uint8_t pin=parseCodename(codename);
+	bool value=digitalRead(pin);
+	messageOut.writeByte(COMMAND_DIGITAL_READ_RE);
+	messageOut.writeByte(codename);
+	messageOut.writeByte(value);
+	messageOut.sendData();
+}
+void RobotMotorBoard::_analogRead(uint8_t codename){
+	uint8_t pin=codenameToAPin(codename);
+	int value=analogRead(pin);
+	messageOut.writeByte(COMMAND_ANALOG_READ_RE);
+	messageOut.writeByte(codename);
+	messageOut.writeInt(value);
+	messageOut.sendData();
+}
+int RobotMotorBoard::IRread(uint8_t num){
+	IRs.selectPin(num-1); //To make consistant with the pins labeled on the board
+	return IRs.getAnalogValue();
+}
+
+void RobotMotorBoard::_readIR(){
+	//Serial.println("readIR");
+	int value;
+	messageOut.writeByte(COMMAND_READ_IR_RE);
+	for(int i=1;i<6;i++){
+		value=IRread(i);
+		messageOut.writeInt(value);
+	}
+	messageOut.sendData();
+}
+
+void RobotMotorBoard::_readTrim(){
+	int value=analogRead(TRIM);
+	messageOut.writeByte(COMMAND_READ_TRIM_RE);
+	messageOut.writeInt(value);
+	messageOut.sendData();	
+}
+
+void RobotMotorBoard::_refreshMotorAdjustment(){
+	motorAdjustment=map(analogRead(TRIM),0,1023,-30,30)/100.0;
+}
+
+void RobotMotorBoard::reportActionDone(){
+	setMode(MODE_SIMPLE);
+	messageOut.writeByte(COMMAND_ACTION_DONE);
+	messageOut.sendData();
+}
+
+RobotMotorBoard RobotMotor=RobotMotorBoard();
+\ No newline at end of file
diff --git a/libraries/Robot_Motor/ArduinoRobotMotorBoard.h b/libraries/Robot_Motor/ArduinoRobotMotorBoard.h
new file mode 100644
index 0000000..c1004c4
--- /dev/null
+++ b/libraries/Robot_Motor/ArduinoRobotMotorBoard.h
@@ -0,0 +1,125 @@
+#ifndef ArduinoRobot_h
+#define ArduinoRobot_h
+
+#include "EasyTransfer2.h"
+#include "Multiplexer.h"
+#include "LineFollow.h"
+//#include "IRremote.h"
+
+#if ARDUINO >= 100
+#include "Arduino.h"
+#else
+#include "WProgram.h"
+#endif
+
+//Command code
+#define COMMAND_SWITCH_MODE 0
+#define COMMAND_RUN	10
+#define COMMAND_MOTORS_STOP	11
+#define COMMAND_ANALOG_WRITE	20
+#define COMMAND_DIGITAL_WRITE	30
+#define COMMAND_ANALOG_READ	40
+#define COMMAND_ANALOG_READ_RE	41
+#define COMMAND_DIGITAL_READ	50
+#define COMMAND_DIGITAL_READ_RE	51
+#define COMMAND_READ_IR	60
+#define COMMAND_READ_IR_RE	61
+#define COMMAND_ACTION_DONE 70
+#define COMMAND_READ_TRIM 80
+#define COMMAND_READ_TRIM_RE 81
+#define COMMAND_PAUSE_MODE 90
+#define COMMAND_LINE_FOLLOW_CONFIG 100
+
+
+//component codename
+#define CN_LEFT_MOTOR	0
+#define CN_RIGHT_MOTOR	1
+#define CN_IR 2
+
+//motor board modes
+#define MODE_SIMPLE 0
+#define MODE_LINE_FOLLOW 1
+#define MODE_ADJUST_MOTOR 2
+#define MODE_IR_CONTROL 3
+
+//bottom TKs, just for communication purpose
+#define B_TK1 201
+#define B_TK2 202
+#define B_TK3 203
+#define B_TK4 204
+
+/*
+A message structure will be:
+switch mode (2):
+	byte COMMAND_SWITCH_MODE, byte mode
+run (5):
+	byte COMMAND_RUN, int speedL, int speedR
+analogWrite (3):
+	byte COMMAND_ANALOG_WRITE, byte codename, byte value;
+digitalWrite (3):
+	byte COMMAND_DIGITAL_WRITE, byte codename, byte value;
+analogRead (2):
+	byte COMMAND_ANALOG_READ, byte codename;
+analogRead _return_ (4):
+	byte COMMAND_ANALOG_READ_RE, byte codename, int value;
+digitalRead (2):
+	byte COMMAND_DIGITAL_READ, byte codename;
+digitalRead _return_ (4):
+	byte COMMAND_DIGITAL_READ_RE, byte codename, int value;
+read IR (1):
+	byte COMMAND_READ_IR;
+read IR _return_ (9):
+	byte COMMAND_READ_IR_RE, int valueA, int valueB, int valueC, int valueD;
+
+
+*/
+
+class RobotMotorBoard:public LineFollow{
+	public:
+		RobotMotorBoard();
+		void begin();
+		
+		void process();
+		
+		void parseCommand();
+		
+		int IRread(uint8_t num);
+		
+		void setMode(uint8_t mode);
+		void pauseMode(bool onOff);
+		
+		void motorsWrite(int speedL, int speedR);
+		void motorsWritePct(int speedLpct, int speedRpct);//write motor values in percentage
+		void motorsStop();		
+	private:
+		float motorAdjustment;//-1.0 ~ 1.0, whether left is lowered or right is lowered
+		
+		//convert codename to actual pins
+		uint8_t parseCodename(uint8_t codename);
+		uint8_t codenameToAPin(uint8_t codename);
+		
+		void stopCurrentActions();
+		//void sendCommand(byte command,byte codename,int value);
+		
+		void _analogWrite(uint8_t codename, int value);
+		void _digitalWrite(uint8_t codename, bool value);
+		void _analogRead(uint8_t codename);
+		void _digitalRead(uint8_t codename);
+		void _readIR();
+		void _readTrim();
+		
+		void _refreshMotorAdjustment();
+		
+		Multiplexer IRs;
+		uint8_t mode;
+		uint8_t isPaused;
+		EasyTransfer2 messageIn;
+		EasyTransfer2 messageOut;
+		
+		//Line Following 
+		void reportActionDone();
+};
+
+extern RobotMotorBoard RobotMotor;
+
+#endif
+\ No newline at end of file
diff --git a/libraries/Robot_Motor/EasyTransfer2.cpp b/libraries/Robot_Motor/EasyTransfer2.cpp
new file mode 100644
index 0000000..24427cc
--- /dev/null
+++ b/libraries/Robot_Motor/EasyTransfer2.cpp
@@ -0,0 +1,152 @@
+#include "EasyTransfer2.h"
+
+
+
+
+//Captures address and size of struct
+void EasyTransfer2::begin(HardwareSerial *theSerial){
+	_serial = theSerial;
+	
+	//dynamic creation of rx parsing buffer in RAM
+	//rx_buffer = (uint8_t*) malloc(size);
+	
+	resetData();
+}
+
+void EasyTransfer2::writeByte(uint8_t dat){
+	if(position<20)
+		data[position++]=dat;
+		size++;
+}
+void EasyTransfer2::writeInt(int dat){
+	if(position<19){
+		data[position++]=dat>>8;
+		data[position++]=dat;
+		size+=2;
+	}
+}
+uint8_t EasyTransfer2::readByte(){
+	if(position>=size)return 0;
+	return data[position++];
+}
+int EasyTransfer2::readInt(){
+	if(position+1>=size)return 0;
+	int dat_1=data[position++]<<8;
+	int dat_2=data[position++];
+	int dat= dat_1 | dat_2;
+	return dat;
+}
+
+void EasyTransfer2::resetData(){
+	for(int i=0;i<20;i++){
+		data[i]=0;
+	}
+	size=0;
+	position=0;
+}
+
+//Sends out struct in binary, with header, length info and checksum
+void EasyTransfer2::sendData(){
+  uint8_t CS = size;
+  _serial->write(0x06);
+  _serial->write(0x85);
+  _serial->write(size);
+  for(int i = 0; i<size; i++){
+    CS^=*(data+i);
+    _serial->write(*(data+i));
+	//Serial.print(*(data+i));
+	//Serial.print(",");
+  }
+  //Serial.println("");
+  _serial->write(CS);
+  
+  resetData();
+}
+
+boolean EasyTransfer2::receiveData(){
+  
+  //start off by looking for the header bytes. If they were already found in a previous call, skip it.
+  if(rx_len == 0){
+  //this size check may be redundant due to the size check below, but for now I'll leave it the way it is.
+    if(_serial->available() >= 3){
+	//this will block until a 0x06 is found or buffer size becomes less then 3.
+      while(_serial->read() != 0x06) {
+		//This will trash any preamble junk in the serial buffer
+		//but we need to make sure there is enough in the buffer to process while we trash the rest
+		//if the buffer becomes too empty, we will escape and try again on the next call
+		if(_serial->available() < 3)
+			return false;
+		}
+		//Serial.println("head");
+      if (_serial->read() == 0x85){
+        rx_len = _serial->read();
+		//Serial.print("rx_len:");
+		//Serial.println(rx_len);
+		resetData();
+
+		//make sure the binary structs on both Arduinos are the same size.
+        /*if(rx_len != size){
+          rx_len = 0;
+          return false;
+        }*/
+      }
+    }
+	//Serial.println("nothing");
+  }
+  
+  //we get here if we already found the header bytes, the struct size matched what we know, and now we are byte aligned.
+  if(rx_len != 0){
+	
+    while(_serial->available() && rx_array_inx <= rx_len){
+      data[rx_array_inx++] = _serial->read();
+    }
+    
+    if(rx_len == (rx_array_inx-1)){
+      //seem to have got whole message
+      //last uint8_t is CS
+      calc_CS = rx_len;
+	  //Serial.print("len:");
+	  //Serial.println(rx_len);
+      for (int i = 0; i<rx_len; i++){
+        calc_CS^=data[i];
+		//Serial.print("m");
+		//Serial.print(data[i]);
+		//Serial.print(",");
+      } 
+	  //Serial.println();
+      //Serial.print(data[rx_array_inx-1]);
+	  //Serial.print(" ");
+	  //Serial.println(calc_CS);
+	  
+      if(calc_CS == data[rx_array_inx-1]){//CS good
+		//resetData();
+        //memcpy(data,d,rx_len);
+		for(int i=0;i<20;i++){
+			//Serial.print(data[i]);
+			//Serial.print(",");
+		}
+		//Serial.println("");
+		size=rx_len;
+		rx_len = 0;
+		rx_array_inx = 0;
+		return true;
+		}
+		
+	  else{
+  		//Serial.println("CS");
+		resetData();
+	  //failed checksum, need to clear this out anyway
+		rx_len = 0;
+		rx_array_inx = 0;
+		return false;
+	  }
+        
+    }
+  }
+  //Serial.print(rx_len);
+  //Serial.print(" ");
+  //Serial.print(rx_array_inx);
+  //Serial.print(" ");
+  //Serial.println("Short");
+  return false;
+}
+\ No newline at end of file
diff --git a/libraries/Robot_Motor/EasyTransfer2.h b/libraries/Robot_Motor/EasyTransfer2.h
new file mode 100644
index 0000000..3369a51
--- /dev/null
+++ b/libraries/Robot_Motor/EasyTransfer2.h
@@ -0,0 +1,76 @@
+/******************************************************************
+*  EasyTransfer Arduino Library 
+*		details and example sketch: 
+*			http://www.billporter.info/easytransfer-arduino-library/
+*
+*		Brought to you by:
+*              Bill Porter
+*              www.billporter.info
+*
+*		See Readme for other info and version history
+*	
+*  
+*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.
+<http://www.gnu.org/licenses/>
+*
+*This work is licensed under the Creative Commons Attribution-ShareAlike 3.0 Unported License. 
+*To view a copy of this license, visit http://creativecommons.org/licenses/by-sa/3.0/ or
+*send a letter to Creative Commons, 444 Castro Street, Suite 900, Mountain View, California, 94041, USA.
+******************************************************************/
+#ifndef EasyTransfer2_h
+#define EasyTransfer2_h
+
+
+//make it a little prettier on the front end. 
+#define details(name) (byte*)&name,sizeof(name)
+
+//Not neccessary, but just in case. 
+#if ARDUINO > 22
+#include "Arduino.h"
+#else
+#include "WProgram.h"
+#endif
+#include "HardwareSerial.h"
+//#include <NewSoftSerial.h>
+#include <math.h>
+#include <stdio.h>
+#include <stdint.h>
+#include <avr/io.h>
+
+class EasyTransfer2 {
+public:
+void begin(HardwareSerial *theSerial);
+//void begin(uint8_t *, uint8_t, NewSoftSerial *theSerial);
+void sendData();
+boolean receiveData();
+
+void writeByte(uint8_t dat);
+void writeInt(int dat);
+uint8_t readByte();
+int readInt();
+
+
+private:
+HardwareSerial *_serial;
+
+void resetData();
+
+uint8_t data[20];	//data storage, for both read and send
+uint8_t position;
+uint8_t size; //size of data in bytes. Both for read and send
+//uint8_t * address;  //address of struct
+//uint8_t size;       //size of struct
+//uint8_t * rx_buffer; //address for temporary storage and parsing buffer
+//uint8_t rx_buffer[20];
+uint8_t rx_array_inx;  //index for RX parsing buffer
+uint8_t rx_len;		//RX packet length according to the packet
+uint8_t calc_CS;	   //calculated Chacksum
+};
+
+
+
+#endif
+\ No newline at end of file
diff --git a/libraries/Robot_Motor/LineFollow.h b/libraries/Robot_Motor/LineFollow.h
new file mode 100644
index 0000000..608d573
--- /dev/null
+++ b/libraries/Robot_Motor/LineFollow.h
@@ -0,0 +1,40 @@
+#ifndef LINE_FOLLOW_H
+#define LINE_FOLLOW_H
+
+#if ARDUINO >= 100
+	#include "Arduino.h"
+#else
+	#include "WProgram.h"
+#endif
+
+class LineFollow{
+	public:
+		LineFollow();
+
+		void calibIRs();
+		void runLineFollow();
+		void config(uint8_t KP, uint8_t KD, uint8_t robotSpeed, uint8_t intergrationTime);
+		
+		//These are all pure virtual functions, pure VF needs pure specifier "=0"
+		//virtual void motorsWrite(int speedL, int speedR)=0;
+		virtual void motorsWritePct(int speedLpct, int speedRpct)=0;
+		virtual void motorsStop()=0;
+		virtual int IRread(uint8_t num)=0;
+	protected:
+		virtual	void reportActionDone()=0;
+		
+	private:
+		void doCalibration(int speedPct, int time);
+		void ajusta_niveles();
+		
+		uint8_t KP;
+		uint8_t KD;
+		uint8_t robotSpeed; //percentage
+		uint8_t intergrationTime;
+		
+		int lectura_sensor[5], last_error, acu;
+		int sensor_blanco[5];
+		int sensor_negro[5];
+};
+
+#endif
+\ No newline at end of file
diff --git a/libraries/Robot_Motor/Multiplexer.cpp b/libraries/Robot_Motor/Multiplexer.cpp
new file mode 100644
index 0000000..c0fdd86
--- /dev/null
+++ b/libraries/Robot_Motor/Multiplexer.cpp
@@ -0,0 +1,37 @@
+#include "Multiplexer.h"
+
+void Multiplexer::begin(uint8_t* selectors, uint8_t Z, uint8_t length){
+	for(uint8_t i=0;i<length;i++){
+		this->selectors[i]=selectors[i];
+		pinMode(selectors[i],OUTPUT);
+	}
+	this->length=length;
+	this->pin_Z=Z;
+	pinMode(pin_Z,INPUT);
+}
+
+void Multiplexer::selectPin(uint8_t num){
+	for(uint8_t i=0;i<length;i++){
+		//Serial.print(bitRead(num,i));
+		digitalWrite(selectors[i],bitRead(num,i));
+	}
+	//Serial.println("");
+}
+
+int Multiplexer::getAnalogValue(){
+	return analogRead(pin_Z);
+}
+
+bool Multiplexer::getDigitalValue(){
+	return digitalRead(pin_Z);
+}
+
+int Multiplexer::getAnalogValueAt(uint8_t num){
+	selectPin(num);
+	return getAnalogValue();
+}
+
+bool Multiplexer::getDigitalValueAt(uint8_t num){
+	selectPin(num);
+	return getDigitalValue();
+}
+\ No newline at end of file
diff --git a/libraries/Robot_Motor/Multiplexer.h b/libraries/Robot_Motor/Multiplexer.h
new file mode 100644
index 0000000..a0c4c5c
--- /dev/null
+++ b/libraries/Robot_Motor/Multiplexer.h
@@ -0,0 +1,24 @@
+#ifndef Multiplexer_h
+#define Multiplexer_h
+
+#if ARDUINO >= 100
+#include "Arduino.h"
+#else
+#include "WProgram.h"
+#endif
+
+class Multiplexer{
+	public:
+		void begin(uint8_t* selectors, uint8_t Z, uint8_t length);
+		void selectPin(uint8_t num);
+		int getAnalogValue();
+		int getAnalogValueAt(uint8_t num);
+		bool getDigitalValue();
+		bool getDigitalValueAt(uint8_t num);
+	private:
+		uint8_t selectors[4];
+		uint8_t pin_Z;
+		uint8_t length;
+};
+
+#endif
diff --git a/libraries/Robot_Motor/examples/Robot_IR_Array_Test/Robot_IR_Array_Test.ino b/libraries/Robot_Motor/examples/Robot_IR_Array_Test/Robot_IR_Array_Test.ino
new file mode 100644
index 0000000..e201fd9
--- /dev/null
+++ b/libraries/Robot_Motor/examples/Robot_IR_Array_Test/Robot_IR_Array_Test.ino
@@ -0,0 +1,26 @@
+/* Motor Board IR Array Test
+   
+   This example of the Arduno robot's motor board returns the 
+   values read fron the 5 infrared sendors on the bottom of 
+   the robot.
+
+*/
+// include the motor board header 
+#include <ArduinoRobotMotorBoard.h>
+
+String bar; // string for storing the informaton
+
+void setup(){
+  // start serial communication
+  Serial.begin(9600);
+  // initialize the library
+  RobotMotor.begin();
+}
+void loop(){
+  bar=String(""); // empty the string
+  // read the sensors and add them to the string
+  bar=bar+RobotMotor.readIR(0)+' '+RobotMotor.readIR(1)+' '+RobotMotor.readIR(2)+' '+RobotMotor.readIR(3)+' '+RobotMotor.readIR(4);
+  // print out the values
+  Serial.println(bar);
+  delay(100);
+}
diff --git a/libraries/Robot_Motor/examples/Robot_Motor_Core/Robot_Motor_Core.ino b/libraries/Robot_Motor/examples/Robot_Motor_Core/Robot_Motor_Core.ino
new file mode 100644
index 0000000..f74f30a
--- /dev/null
+++ b/libraries/Robot_Motor/examples/Robot_Motor_Core/Robot_Motor_Core.ino
@@ -0,0 +1,18 @@
+/* Motor Core
+
+  This code for the Arduino Robot's motor board
+  is the stock firmware. program the motor board with 
+  this sketch whenever you want to return the motor
+  board to its default state.
+  
+*/
+
+#include <ArduinoRobotMotorBoard.h>
+
+void setup(){
+  RobotMotor.begin();
+}
+void loop(){
+  RobotMotor.parseCommand();
+  RobotMotor.process();
+}
diff --git a/libraries/Robot_Motor/lineFollow.cpp b/libraries/Robot_Motor/lineFollow.cpp
new file mode 100644
index 0000000..d6ebed8
--- /dev/null
+++ b/libraries/Robot_Motor/lineFollow.cpp
@@ -0,0 +1,152 @@
+//#include <ArduinoRobotMotorBoard.h>
+#include "LineFollow.h"
+
+//#define KP 19 //0.1 units
+//#define KD 14
+//#define ROBOT_SPEED 100 //percentage
+
+//#define KP 11
+//#define KD 5
+//#define ROBOT_SPEED 50
+
+//#define INTEGRATION_TIME 10 //En ms
+
+/*uint8_t KP=11;
+uint8_t KD=5;
+uint8_t robotSpeed=50; //percentage
+uint8_t intergrationTime=10;*/
+
+#define NIVEL_PARA_LINEA 50 
+
+/*int lectura_sensor[5], last_error=0, acu=0;
+
+//Estos son los arrays que hay que rellenar con los valores de los sensores
+//de suelo sobre blanco y negro. 
+int sensor_blanco[]={
+  0,0,0,0,0};
+int sensor_negro[]={
+  1023,1023,1023,1023,1023};
+*/
+//unsigned long time;
+
+//void mueve_robot(int vel_izq, int vel_der);
+//void para_robot();
+//void doCalibration(int speedPct, int time);
+//void ajusta_niveles();	//calibrate values
+
+LineFollow::LineFollow(){
+	/*KP=11;
+	KD=5;
+	robotSpeed=50; //percentage
+	intergrationTime=10;*/
+	config(11,5,50,10);
+		
+	for(int i=0;i<5;i++){
+		sensor_blanco[i]=0;
+		sensor_negro[i]=1023;
+	}
+}
+
+void LineFollow::config(uint8_t KP, uint8_t KD, uint8_t robotSpeed, uint8_t intergrationTime){
+	this->KP=KP;
+	this->KD=KD;
+	this->robotSpeed=robotSpeed;
+	this->intergrationTime=intergrationTime;
+	/*Serial.print("LFC: ");
+	Serial.print(KP);
+	Serial.print(' ');
+	Serial.print(KD);
+	Serial.print(' ');
+	Serial.print(robotSpeed);
+	Serial.print(' ');
+	Serial.println(intergrationTime);*/
+	
+}
+void LineFollow::calibIRs(){
+	static bool isInited=false;//So only init once
+	if(isInited)return ;
+	
+	delay(1000);
+	
+	doCalibration(30,500);
+	doCalibration(-30,800);
+	doCalibration(30,500);
+	
+	delay(1000);
+	isInited=true;
+}
+
+void LineFollow::runLineFollow(){
+  for(int count=0; count<5; count++)
+  {
+    lectura_sensor[count]=map(IRread(count),sensor_negro[count],sensor_blanco[count],0,127);
+    acu+=lectura_sensor[count];
+  }
+
+  //Serial.println(millis());
+  if (acu > NIVEL_PARA_LINEA)
+  {
+    acu/=5;
+
+    int error = ((lectura_sensor[0]<<6)+(lectura_sensor[1]<<5)-(lectura_sensor[3]<<5)-(lectura_sensor[4]<<6))/acu;
+
+    error = constrain(error,-100,100);
+
+    //Calculamos la correcion de velocidad mediante un filtro PD
+    int vel = (error * KP)/10 + (error-last_error)*KD;
+
+    last_error = error;
+
+    //Corregimos la velocidad de avance con el error de salida del filtro PD
+    int motor_left = constrain((robotSpeed + vel),-100,100);
+    int motor_right =constrain((robotSpeed - vel),-100,100);
+
+    //Movemos el robot
+    //motorsWritePct(motor_left,motor_right);
+    motorsWritePct(motor_left,motor_right);
+
+    //Esperamos un poquito a que el robot reaccione
+    delay(intergrationTime);
+  }
+  else
+  {
+    //Hemos encontrado una linea negra
+    //perpendicular a nuestro camino
+    //paramos el robot
+    motorsStop();
+
+    //y detenemos la ejecuci�n del programa
+    //while(true);
+	reportActionDone();
+	//setMode(MODE_SIMPLE);
+  }
+}  
+
+
+void LineFollow::doCalibration(int speedPct, int time){
+	motorsWritePct(speedPct, -speedPct);
+	unsigned long beginTime = millis();
+	while((millis()-beginTime)<time)
+		ajusta_niveles();
+	motorsStop();
+}
+void LineFollow::ajusta_niveles()
+{
+	int lectura=0;
+
+	for(int count=0; count<5; count++){
+		lectura=IRread(count);
+
+		if (lectura > sensor_blanco[count])
+			sensor_blanco[count]=lectura;
+
+		if (lectura < sensor_negro[count])
+			sensor_negro[count]=lectura;
+	} 
+}
+
+
+
+
+
+