Código Arduino

monkeyserna edited this page May 11, 2014 · 36 revisions

La placa Arduino recibe por puerto USB un comando de 3 bytes de longitud, donde el primer byte indica el comando, y los dos restantes están destinados a parámetros. Estos parámetros, al ser de tipo byte, contemplan un rango numérico del 0 al 255.

Hay 17 comandos disponibles. Los 11 primeros están relacionados con el movimiento de los motores (comandos 0 al 10). El comando 11 está destinado al zumbador, el 12 al encendido y apagado de los leds, y los 4 últimos (13 al 16) realizan distintas lecturas de los sensores infrarrojos CNY70.

A continuación se detallan las funciones del código.

Funciones básicas Arduino

Inicialización

La inicialización del código se realiza en tres apartados. La definición de constantes se realiza en el archivo Types.h. La declaración de variables globales se situa al comienzo del archivo principal Arduino.ino. Por último, la inicialización se realiza en la función void setup().

Types.h

En este fichero se declaran las constantes empleadas en el código fuente.

Se dividen en las siguientes categorías

  • Constantes generales
    • KMAXSPEED 400: Velocidad máxima a la que se pueden establecer los motores.
    • KSTRAIGHTSPEED 300: Velocidad máxima para el modo de movimiento de velocidad-balance.
    • KTURNSPEED 200: Velocidad de giro máxima para el modo de movimiento de velocidad-balance.
    • MAX_ANALOG_RESOLUTION 1023: Resolución de lectura analógica.
    • FRONTAL_TIMEOUT 2000: Tiempo máximo de lectura de los infrarrojos analógicos.
    • CNY70DIVIDER 30: Valor a partir del cual un sensor infrarrojo de lectura analógica se considera negro.
  • Entradas
    • NUMINPUTS 22: Número total de elementos de entrada.
    • I_BUTTON 13: Pin del botón. No se utiliza.
    • Matrices
      • O_IRON_AN 29: Pin de la matriz de sensores frontales. Para activar o desactivar todos los sensores frontales. Esto reduce el consumo eléctrico del robot.
      • O_IRON_DG 27: Pin de la matriz de sensores centrales.
    • Sensores frontales
      • NUMFRONTS 12: Número de sensores frontales.
      • I_IR10 53: Pin del sensor frontal 1 (a la izquierda).
      • I_IR11 51: Pin del sensor frontal 2.
      • I_IR12 49: Pin del sensor frontal 3.
      • I_IR13 47: Pin del sensor frontal 4.
      • I_IR14 45: Pin del sensor frontal 5.
      • I_IR15 43: Pin del sensor frontal 6.
      • I_IR16 41: Pin del sensor frontal 7.
      • I_IR17 39: Pin del sensor frontal 8.
      • I_IR18 37: Pin del sensor frontal 9.
      • I_IR19 35: Pin del sensor frontal 10.
      • I_IR20 33: Pin del sensor frontal 11.
      • I_IR21 31: Pin del sensor frontal 12 (a la derecha).
    • Sensores centrales
      • NUMMIDS 9: Número de sensores centrales.
      • I_IR1 4: Pin del sensor central 1 (a la izquierda).
      • I_IR2 5: Pin del sensor central 2.
      • I_IR3 A0: Pin del sensor central 3.
      • I_IR4 A1: Pin del sensor central 4.
      • I_IR5 6: Pin del sensor central 5.
      • I_IR6 A2: Pin del sensor central 6.
      • I_IR7 A3: Pin del sensor central 7.
      • I_IR8 11: Pin del sensor central 8.
      • I_IR9 12: Pin del sensor central 9 (a la derecha).
  • Salidas
    • NUMOUTPUTS 21: Número total de elementos de salida.
    • O_BUZZER 23: Pin del zumbador.
    • LEDs
      • O_LED4 52: Pin del LED 1 (a la izquierda).
      • O_LED5 50: Pin del LED 2.
      • O_LED6 48: Pin del LED 3.
      • O_LED7 46: Pin del LED 4.
      • O_LED8 44: Pin del LED 5.
      • O_LED9 42: Pin del LED 6.
      • O_LED10 36: Pin del LED 7.
      • O_LED11 34: Pin del LED 8.
      • O_LED12 32: Pin del LED 9.
      • O_LED13 30: Pin del LED 10.
      • O_LED14 28: Pin del LED 11.
      • O_LED15 26: Pin del LED 12 (a la derecha).
      • O_LED16 24: Pin del LED central izquierdo.
      • O_LED17 22: Pin del LED central derecho.
    • Motores
      • O_Ain1 9: Pin de la entrada 1 del motor derecho.
      • O_Ain2 7: Pin de la entrada 2 del motor derecho.
      • O_Bin1 10: Pin de la entrada 1 del motor izquierdo.
      • O_Bin2 8: Pin de la entrada 2 del motor izquierdo.
  • Listas
    • const byte LEDS[NUMLEDS]: Vector que contiene las definiciones de pines de los LEDs.
    • const byte MIDDLESENSORS[NUMMIDS]: Vector que contiene los sensores centrales.
    • const byte FRONTSENSORS[NUMFRONTS]: Vector que contiene los sensores frontales.
    • const byte INPUTS [NUMINPUTS]}: Vector que contiene todos los elementos de entrada.
    • const byte OUTPUTS [NUMOUTPUTS]: Vector que contiene todos los elementos de salida.
Código fuente
#ifndef _GR_TYPES
#define _GR_TYPES
#include "Arduino.h"

/**********************************************/
//
//            SYSTEM CONSTANTS
//
/**********************************************/

#define KMAXSPEED       400
#define KSTRAIGHTSPEED  300
#define KTURNSPEED      200


#define MAX_ANALOG_RESOLUTION 1023

#define FRONTAL_TIMEOUT 2000
#define CNY70DIVIDER 30

//-------------------------
//        INPUTS
//-------------------------

#define NUMINPUTS 22

//Push button (Pull-up)
#define  I_BUTTON	13


//On/off infrared sensors

#define  O_IRON_AN	29   // Analogical infrared (Pull-Up)
#define  O_IRON_DG	27   // Digital infrared (Pull-Down)

//Analog infrared sensors

#define NUMFRONTS  12

#define I_IR10	53
#define I_IR11	51
#define I_IR12	49
#define I_IR13	47
#define I_IR14	45
#define I_IR15	43
#define I_IR16	41
#define I_IR17	39
#define I_IR18	37
#define I_IR19	35
#define I_IR20	33
#define I_IR21	31

//Digital infrared sensors

#define NUMMIDS  9

#define I_IR1	4
#define I_IR2	5
#define I_IR3	A0
#define I_IR4	A1
#define I_IR5	6
#define I_IR6	A2
#define I_IR7	A3
#define I_IR8	11
#define I_IR9	12

//-------------------------
//        OUTPUTS
//-------------------------

#define NUMOUTPUTS 21

//Buzzer (Pull-Down)
#define  O_BUZZER	23


//LEDs

#define  NUMLEDS        14

#define  O_LED4		52
#define  O_LED5		50
#define  O_LED6		48
#define  O_LED7		46
#define  O_LED8		44
#define  O_LED9		42
#define  O_LED10	36
#define  O_LED11	34
#define  O_LED12	32
#define  O_LED13	30
#define  O_LED14	28
#define  O_LED15	26
#define  O_LED16	24
#define  O_LED17	22

//------------------------
//          Motor
//------------------------

//A channel
#define  O_Ain1	9
#define  O_Ain2	7

//B channel
#define  O_Bin1	10
#define  O_Bin2	 8

//------------------------
//          Lists
//------------------------

const byte LEDS[NUMLEDS] = { O_LED4, O_LED5,O_LED6,O_LED7,O_LED9,O_LED10,O_LED11,O_LED12,O_LED14,O_LED15,O_LED16,O_LED17 ,O_LED8, O_LED13 };

const byte MIDDLESENSORS[NUMMIDS] = {  I_IR1, I_IR2, I_IR3, I_IR4, I_IR5, I_IR6, I_IR7, I_IR8, I_IR9};
const byte FRONTSENSORS[NUMFRONTS] = { I_IR10, I_IR11, I_IR12, I_IR13, I_IR14, I_IR15, I_IR16, I_IR17, I_IR18, I_IR19, I_IR20, I_IR21    };

const byte INPUTS [NUMINPUTS]= { I_BUTTON, I_IR1, I_IR2, I_IR3, I_IR4, I_IR5, I_IR6, I_IR7, I_IR8, I_IR9, I_IR10, I_IR11, I_IR12, I_IR13, I_IR14, I_IR15, I_IR16, I_IR17, I_IR18, I_IR19, I_IR20, I_IR21  };
const byte OUTPUTS [NUMOUTPUTS] = { O_IRON_AN, O_IRON_DG , O_BUZZER,  O_LED4, O_LED5,O_LED6,O_LED7,O_LED8,O_LED9,O_LED10,O_LED11,O_LED12,O_LED13,O_LED14,O_LED15,O_LED16,O_LED17,O_Ain1,O_Ain2,O_Bin1,O_Bin2  };

#endif

Variables globales

Para la lectura de sonares se emplean variables globales. Contretamente, se definen las siguientes variables:

  • bool middleSensorValue [NUMMIDS]: Vector de 9 booleanos que contiene los últimos valores leídos de los sensores infrarrojos situados en la posición central. true indica negro y false blanco.
  • unsigned int frontSensorValue [NUMFRONTS]: Vector de 12 enteros sin signo que contiene los últimos valores leídos de los sensores infrarrojos situados en la posición frontal. El rango de valores es de [0,2000], donde 0 indica blanco absoluto y 2000 negro absoluto.
  • bool frontSensorBool [NUMFRONTS]: Vector de 12 booleanos que contiene los últimos valores leídos de los sensores infrarrojos situados en la posición frontal. true indica negro y false blanco.
  • bool isSensorRead [NUMFRONTS]: Vector auxiliar de 12 enteros sin signo empleado para la lectura de los sensores frontales. El uso de esta variable se explica en la función void readFront().

void setup()

Función obligatoria de inicialización del framework Arduino. Debe estar presente en el código, aunque no es necesario que realice operaciones. En caso de no definirse esta función el compilador devolverá un error de compilación.

Parámetros

No recibe parámetros.

Descripción de la función
  • Inicializa el código.

  • Abre la comunicación serie a 9600 baudios para poder realizar la comunicación por USB con la Raspberry Pi, y establece los modos de los pines de entrada y salida.

  • También establece la resolución de la escritura analógica a 10 bytes (esto significa que los valores que se pueden escribir con la función analogWrite(pin,value) van desde 0 hasta 1023). Por defecto, Arduino Due tiene una resolución de 12 bytes, aunque para mantener la compatibilidad con Arduino Mega, la placa GOShieldGR tiene una resolución de 10 bytes. La escritura analógica se emplea en el movimiento de los motores.

Código fuente
void setup()
{
	//Disables interrupts for the initialization
	noInterrupts();

	//Enables USB port for communication with Raspberry
	Serial.begin(9600);

	//Resolution of analogical port: 10 = 1024 (can be configured up to 12 = 4095)
	analogWriteResolution(10);
	
	//Configuration of input pins
	for (int i=0;i< NUMINPUTS  ;i++) 
	{
		pinMode(INPUTS[i],INPUT);
		digitalWrite(INPUTS[i], LOW);
	}
	
	//Configuration of output pins
	for (int i=0;i< NUMOUTPUTS ;i++)
	{
		pinMode(OUTPUTS[i],OUTPUT);
		digitalWrite(OUTPUTS[i], LOW);
	}
	
	//Activates interrupts again
	interrupts();
}

void loop()

Función obligatoria del framework Arduino. Debe estar presente en el código, aunque no es necesario que realice operaciones. En caso de no definirse esta función el compilador devolverá un error de compilación.

Parámetros

No recibe parámetros.

Descripción de la función
  • Es el bucle principal del código. Una vez inicializado, se ejecuta eternamente.

  • Esta función espera a recibir un comando y mediante un switch llama a la función correspondiente.

  • Si hay 3 bytes disponibles en el buffer de la comunicación serie, lee el mensaje y mediante un switch invoca a la función que implementa el comando a ejecutar.

Comandos

Los comandos disponibles son los siguientes:

  1. stopMotors(): Detiene el movimiento de los motores
  2. moveBalanced(speed,balance): Mueve los motores hacia adelante. El primer parámetro que recibe indica la velocidad, y el segundo el balance (0 se corresponde con un giro brusco a la izquierda, 123 avanza hazia adelante y 255 gira bruscamente a la derecha)
  3. setLeftSpeed(speed): Establece la velocidad del motor izquierdo.
  4. setRightSpeed(speed): Establece la velocidad del motor derecho.
  5. moveForwards(speed): Establece la velocidad de ambos motores.
  6. turnLeft(speed): Establece la velocidad del motor izquierdo y detiene el derecho.
  7. turnRight(speed): Establece la velocidad del motor derecho y detiene el izquierdo.
  8. moveBackBalanced(speed,balance): Mueve los motores hacia atrás con velocidad y balance.
  9. moveBackBoth(speed): Establece la velocidad de movimiento hacia atrás de ambos motores.
  10. moveBackLeft(speed): Establece la velocidad de movimiento hacia atrás del motor izquierdo.
  11. moveBackRight(speed): Establece la velocidad de movimiento hacia atrás del motor derecho.
  12. beep(state): Establece el estado (encendido = 1, apagado = 0) del zumbador.
  13. led(numLed, state): Enciende (state = 1) o apaga (state = 0) el led numLed (entre 0 y 13).
  14. sendFrontAndMiddle(): Lee todos los sensores infrarrojos y los envía los valores por USB.
  15. sendFront(): Lee los sensores frontales en binario (negro = 1, blanco = 0) y envía los valores por USB.
  16. sendMiddle(): Lee los sensores centrales en binario (negro = 1, blanco = 0) y envía los valores por USB.
  17. sendAnalogFront(): Lee los sensores frontales en analógico y envía los valores por USB.
Código fuente
void loop()
{
	//Receive a message (3 bytes)
	if (Serial.available() >= 3)
	{
		byte cmd = Serial.read();  //Command
		byte p1  = Serial.read();  //First parameter
		byte p2  = Serial.read();  //Second parameter
		
		switch (cmd)
		{
			//Command 0 -> Stop
			case 0:
			stopMotors();
			break;
			
			//Command 1 -> Move forwards with speed and balance
			case 1:
			moveBalanced(p1,p2);
			break;
			
			//Set left motor to <SPEED>
			case 2:
			setLeftSpeed(p1);
			break;
			
			//Set right motor to <SPEED>
			case 3:
			setRightSpeed(p1);
			break;
			
			//Move forward to <SPEED>
			case 4:
			moveForwards(p1);
			break;
			
			//Set left motor to <SPEED> and right motor to 0
			case 5:
			turnLeft(p1);
			break;
			
			//Set right motor to <SPEED> and left motor to 0
			case 6:
			turnRight(p1);
			break;
			
			//Move backwards with speed and balance
			case 7:
			moveBackBalanced(p1,p2);
			break;
			
			//Set both motors to back speed
			case 8:
			moveBackBoth(p1);
			break;
			
			//Set left motor to back speed
			case 9:
			moveBackLeft(p1);
			break;
			
			//Set right motor to back speed
			case 10:
			moveBackRight(p1);
			break;
			
			//Beep
			case 11:
			beep(p1);
			break;
			
			//LED <0..13> <ON|OFF>
			case 12:
			led(p1,p2);
			break;
			
			//Send all CNY70 values. First front and then middle
			case 13:
			sendFrontAndMiddle();
			break;
			
			//Send front sensor values
			case 14:
			sendFront();
			break;
			
			//Send middle sensor values
			case 15:
			sendMiddle();
			break;
			
			//Send analog values of front sensor
			case 16:
			sendAnalogFront();
		}
	}
}

Funciones de motores

A continuación se explican las funciones asociadas al movimiento de los motores. Cada motor tiene asociadas 2 señales para controlar su movimiento.

La explicación detallada del funcionamiento de los motores está disponible en esta página.

Movimiento hacia adelante

void stopMotors()

Esta función implementa el comando 0.

Puede ser invocada desde Raspberry Pi mediante el mensaje: [0,0,0] (bytes).

Parámetros

No recibe parámetros.

Descripción de la función
  • Realiza un frenado por bloqueo estableciendo las 4 señales de los motores al valor máximo.

  • Este freno es el más rápido, aunque también el más agresivo. Para realizar un frenado gradual (también llamado frenado por descarga o frenado al aire) se puede emplear la función moveForwards(0), que se corresponde con la señal [4,0,0].

Código fuente
void stopMotors()
{
	analogWrite(O_Bin1,MAX_ANALOG_RESOLUTION);
	digitalWrite(O_Bin2,HIGH);
	analogWrite(O_Ain1,MAX_ANALOG_RESOLUTION);
	digitalWrite(O_Ain2,HIGH);
}

void setForwardSpeedRight(int speed)

Función interna, es invocada desde otras funciones, nunca directamente desde la Raspberry ya que primero el valor debe ser escalado desde [0,255] a [0,KMAXSPEED].

Parámetros

Recibe el parámetro:

  1. int speed: Valor de la velocidad a establecer en el motor, en el rango [0,KMAXSPEED].
Descripción de la función
  • Recibe un valor entre 0 y KMAXSPEED. Esta constante es la velocidad máxima de los motores, que por defecto es 400, y como máximo puede ser 1023, que es la resolución de la escritura analógica. Poner los motores a máxima velocidad no es recomendable ya se pueden dañar. La velocidad máxima recomendada es de 750, aunque esto hace la conducción del robot complicada.

  • Establece la primera señal del motor derecho a la velocidad indicada, y la segunda a nivel bajo. Si la velocidad está fuera del rango permitido, establece la misma al valor máximo o mínimo según corresponda.

Código fuente
void setForwardSpeedRight(int speed)
{
	//Check if numbers are out of range
	if      (speed < 0)         speed = 0;
	else if (speed > KMAXSPEED) speed = KMAXSPEED;

	//Set motor speed
	analogWrite(O_Ain1,speed);
	digitalWrite(O_Ain2,LOW);
}

void setForwardSpeedLeft(int speed)

Función interna, es invocada desde otras funciones, nunca directamente desde la Raspberry ya que primero el valor debe ser escalado desde [0,255] a [0,KMAXSPEED].

Parámetros

Recibe el parámetro:

  1. int speed: Valor de la velocidad a establecer en el motor, en el rango [0,KMAXSPEED].
Descripción de la función
  • Recibe un valor entre 0 y KMAXSPEED. Esta constante es la velocidad máxima de los motores, que por defecto es 400, y como máximo puede ser 1023, que es la resolución de la escritura analógica. Poner los motores a máxima velocidad no es recomendable ya se pueden dañar. La velocidad máxima recomendada es de 750, aunque esto hace la conducción del robot complicada.

  • Establece la primera señal del motor izquierdo a la velocidad indicada, y la segunda a nivel bajo. Si la velocidad está fuera del rango permitido, establece la misma al valor máximo o mínimo según corresponda.

Código fuente
void setForwardSpeedLeft(int speed)
{
	//Check if numbers are out of range
	if      (speed < 0)         speed = 0;
	else if (speed > KMAXSPEED) speed = KMAXSPEED;

	//Set motor speed
	analogWrite(O_Bin1,speed);
	digitalWrite(O_Bin2,LOW);
}

setMotors(int left, int right)

Función interna, es invocada desde otras funciones, nunca directamente desde la Raspberry ya que primero el valor debe ser escalado desde [0,255] a [0,KMAXSPEED].

Parámetros

Recibe los parámetros:

  1. int left Velocidad del motor izquierdo entre [0,KMAXSPEED].
  2. int right Velocidad del motor dercho entre [0,KMAXSPEED].
Descripción de la función
  • Establece la velocidad de los motores haciendo uso de las funciones auxiliares setForwardSpeedRight(left) y setForwardSpeedLeft(right).
Código fuente
void setMotors(int left, int right)
{
	setForwardSpeedRight(left);
	setForwardSpeedLeft(right);
}

void turnLeft(byte speed)

Esta función implementa el comando 5.

Puede ser invocada desde Raspberry Pi mediante el comando: [5,<speed>,0] (bytes).

Parámetros

Recibe el parámetro:

  1. byte speed: Velocidad en el rango [0,255] a la que se establece el motor izquierdo.
Descripción de la función

Escala el valor recibido en el parámetro speed de [0,255] a [0,KMAXSPEED] y establece la velocidad del motor izquierdo. Además, establece a 0 la velocidad del motor derecho, esto garantiza que (en caso de proporcionar una velocidad de giro suficiente) se realice el giro.

Código fuente
void turnLeft(byte speed)
{
	int speedMapped = map(speed,0,255,0,KMAXSPEED);
	setForwardSpeedRight(0);
	setForwardSpeedLeft(speedMapped);
}

void turnRight(byte speed)

Esta función implementa el comando 6.

Puede ser invocada desde Raspberry Pi mediante el comando: [6,<speed>,0] (bytes).

Parámetros

Recibe el parámetro:

  1. byte speed: Velocidad en el rango [0,255] a la que se establece el motor derecho.
Descripción de la función

Escala el valor recibido en el parámetro speed de [0,255] a [0,KMAXSPEED] y establece la velocidad del motor derecho. Además, establece a 0 la velocidad del motor izquierdo, esto garantiza que (en caso de proporcionar una velocidad de giro suficiente) se realice el giro.

Código fuente
void turnRight(byte speed)
{
	int speedMapped = map(speed,0,255,0,KMAXSPEED);
	setForwardSpeedRight(speedMapped);
	setForwardSpeedLeft(0);
}

void setLeftSpeed(byte speed)

Esta función implementa el comando 2.

Puede ser invocada desde Raspberry Pi mediante el comando: [2,<speed>,0] (bytes).

Parámetros

Recibe el parámetro:

  1. byte speed: Velocidad en el rango [0,255] a la que se establece el motor izquierdo.
Descripción de la función

Escala el valor recibido en el parámetro speed desde el rango [0,255] hasta [0,KMAXSPEED].

Establece la velocidad del motor izquierdo a la velocidad, una vez escalada.

Establece la velocidad del motor derecho a 0, garantizando que el robot realizará un giro siempre y cuando la velocidad de giro definida sea suficiente.

Código fuente
void setLeftSpeed(byte speed)
{
	int speedMapped = map(speed,0,255,0,KMAXSPEED);
	setForwardSpeedLeft(speedMapped);
}

void setRightSpeed(byte speed)

Esta función implementa el comando 3.

Puede ser invocada desde Raspberry Pi mediante el comando: [3,<speed>,0] (bytes).

Recibe el parámetro:

  1. byte speed: Velocidad en el rango [0,255] a la que se establece el motor derecho.
Descripción de la función

Escala el valor recibido en el parámetro speed desde el rango [0,255] hasta [0,KMAXSPEED].

Establece la velocidad del motor derecho a la velocidad indicada, una vez escalada.

Establece la velocidad del motor izquierdo a 0, garantizando que el robot realizará un giro siempre y cuando la velocidad de giro definida sea suficiente.

Código fuente
void setRightSpeed(byte speed)
{
	int speedMapped = map(speed,0,255,0,KMAXSPEED);
	setForwardSpeedRight(speedMapped);
}

void moveBalanced(byte speed, byte balance)

Esta función implementa el comando 1.

Puede ser invocada desde Raspberry Pi mediante el comando: [1,<speed>,<balance>] (bytes).

Parámetros

Recibe los parámetros:

  1. byte speed: Velocidad de avance dentro del rango [0,255].
  2. byte balance: Balance del avance dentro del rango [0,255].
  • 0 Implica un giro brusco a la izquierda.
  • 123: Implica avanzar hacia adelante.
  • 255: Implica un giro brusco a la derecha.
Descripción de la función

Escala el valor recibido en el parámetro speed desde [0,255] hasta [0,KSTRAIGHTSPEED].

Escala el valor recibido en el parámetro balance desde [0,255] hasta [0,KTURNSPEED].

Establece la velocidad del motor izquierdo restando a la velocidad de avance (una vez escalada) la velocidad de giro (también escalada).

Establece la velocidad del motor derecho sumando a la velocidad de avance (una vez escalada) la velocidad de giro (también escalada).

Código fuente
void moveBalanced(byte speed, byte balance)
{
	//Sets speed between 0 and KMAXSPEED
	int speedMapped = map(speed,0,255,0,KSTRAIGHTSPEED);

	//Sets balance between -KTURNSPEED and KTURNSPEED
	int balanceMapped = map(balance,0,255,-KTURNSPEED,KTURNSPEED);

	//Sets speed of left and right motors
	int left = speedMapped - balanceMapped;
	int right = speedMapped + balanceMapped;

	setMotors(left,right);
}

void moveForwards(byte speed)

Esta función implementa el comando 4.

Puede ser invocada desde Raspberry Pi mediante el comando: [4,<speed>,0] (bytes).

Parámetros

Recibe el parámetro:

  1. byte speed: Velocidad en el rango [0,255] a la que se establecen los motores.
Descripción de la función

Escala el valor recibido en el parámetro speed desde [0,255] hasta [0,KMAXSPEED].

Establece la velocidad de ambos motores a la velocidad indicada, una vez escalada.

Código fuente
void moveForwards(byte speed)
{
	//Sets speed between 0 and KMAXSPEED
	int speedMapped = map(speed,0,255,0,KMAXSPEED);

	setMotors(speedMapped,speedMapped);
}

Movimiento hacia atrás

void setBackSpeedLeft(int speed)

Función interna, es invocada desde otras funciones, nunca directamente desde la Raspberry ya que primero el valor debe ser escalado desde [0,255] a [0,KMAXSPEED].

Parámetros

Recibe el parámetro

  1. int speed: Valor de la velocidad a establecer en el motor, en el rango [0,KMAXSPEED].
Descripción de la función

Recibe un valor entre 0 y KMAXSPEED. Esta constante es la velocidad máxima de los motores, que por defecto es 400, y como máximo puede ser 1023, que es la resolución de la escritura analógica. Poner los motores a máxima velocidad no es recomendable ya se pueden dañar. La velocidad máxima recomendada es de 750, aunque esto hace la conducción del robot complicada.

Establece la primera señal del motor izquierdo a la velocidad indicada de forma inversa (restando el valor máximo valor permitido, definido en MAX_ANALOG_RESOLUTION como 1023 que es el valor máximo que se puede escribir con la función analogWrite a una resolución de 10 bits. La segunda a nivel alto. Si la velocidad está fuera del rango permitido, establece la misma al valor máximo o mínimo según corresponda.

Código fuente
void setBacSpeedLeft(int speed)
{
	//Check if numbers are out of range
	if      (speed < 0)         speed = 0;
	else if (speed > KMAXSPEED) speed = KMAXSPEED;

	//Set motor speed
	analogWrite(O_Bin1,MAX_ANALOG_RESOLUTION-speed);
	digitalWrite(O_Bin2,HIGH);
}

void setBackSpeedRight(int speed)

Función interna, es invocada desde otras funciones, nunca directamente desde la Raspberry ya que primero el valor debe ser escalado desde [0,255] a [0,KMAXSPEED].

Parámetros

Recibe el parámetro

  1. int speed: Valor de la velocidad a establecer en el motor, en el rango [0,KMAXSPEED].
Descripción de la función

Recibe un valor entre 0 y KMAXSPEED. Esta constante es la velocidad máxima de los motores, que por defecto es 400, y como máximo puede ser 1023, que es la resolución de la escritura analógica. Poner los motores a máxima velocidad no es recomendable ya se pueden dañar. La velocidad máxima recomendada es de 750, aunque esto hace la conducción del robot complicada.

Establece la primera señal del motor derecho a la velocidad indicada de forma inversa (restando el valor máximo valor permitido, definido en MAX_ANALOG_RESOLUTION como 1023 que es el valor máximo que se puede escribir con la función analogWrite a una resolución de 10 bits. La segunda a nivel alto. Si la velocidad está fuera del rango permitido, establece la misma al valor máximo o mínimo según corresponda.

Código fuente
void setBackSpeedRight(int speed)
{
	//Check if numbers are out of range
	if      (speed < 0)         speed = 0;
	else if (speed > KMAXSPEED) speed = KMAXSPEED;

	//Set motor speed
	analogWrite(O_Ain1,MAX_ANALOG_RESOLUTION-speed);
	digitalWrite(O_Ain2,HIGH);
}

void moveBackBalanced(byte speed, byte balance)

Esta función implementa el comando 7.

Puede ser invocada desde Raspberry Pi mediante el comando: [7,<speed>,<balance>] (bytes).

Parámetros

Recibe los parámetros:

  1. byte speed: Velocidad de retroceso dentro del rango [0,255].
  2. byte balance: Balance del retroceso dentro del rango [0,255].
  • 0 implica un giro brusco a la izquierda.
  • 123 implica retroceder.
  • 255 implica un giro brusco a la derecha.
Descripción de la función

Escala el valor recibido en el parámetro speed desde [0,255] hasta [0,KSTRAIGHTSPEED].

Escala el valor recibido en el parámetro balance desde [0,255] hasta [0,KTURNSPEED].

Establece la velocidad del motor izquierdo restando a la velocidad de avance (una vez escalada) la velocidad de giro (también escalada).

Establece la velocidad del motor derecho sumando a la velocidad de avance (una vez escalada) la velocidad de giro (también escalada).

Código fuente
void moveBackBalanced(byte speed, byte balance)
{
	//Sets speed between 0 and KMAXSPEED
	int speedMapped = map(speed,0,255,0,KSTRAIGHTSPEED);

	//Sets balance between -KTURNSPEED and KTURNSPEED
	int balanceMapped = map(balance,0,255,-KTURNSPEED,KTURNSPEED);

	//Sets speed of left and right motors
	int left = speedMapped - balanceMapped;
	int right = speedMapped + balanceMapped;

	setBackwardSpeedLeft(left);
	setBackwardSpeedRight(right);
}

void moveBackBoth(byte speed)

Esta función implementa el comando 8.

Puede ser invocada desde Raspberry Pi mediante el comando: [8,<speed>,0] (bytes).

Parámetros

Recibe el parámetro:

  1. byte speed: Velocidad de retroceso dentro del rango [0,255].
Descripción de la función

Escala el valor recibido en el parámetro speed desde [0,255] hasta [0,KMAXSPEED] y establece con este valor la velocidad de retroceso de ambos motores.

Código fuente
void moveBackBoth(byte speed)
{
	//Sets speed between 0 and KMAXSPEED
	int kspeed = map(speed,0,255,0,KMAXSPEED);
	setBackwardSpeedLeft(kspeed);
	setBackwardSpeedRight(kspeed);
}

void moveBackLeft(byte speed)

Esta función implementa el comando ``.

Puede ser invocada desde Raspberry Pi mediante el comando: [,,] (bytes).

Parámetros

Recibe el parámetro:

  1. byte speed: Velocidad de retroceso dentro del rango [0,255].
Descripción de la función

Escala el valor recibido en el parámetro speed desde [0,255] hasta [0,KMAXSPEED] y establece con este valor la velocidad de retroceso del motor izquierdo.

Código fuente
void moveBackLeft(byte speed)
{
	//Sets speed between 0 and KMAXSPEED
	setBackwardSpeedLeft(map(speed,0,255,0,KMAXSPEED));
}

void moveBackRight(byte speed)

Esta función implementa el comando 10.

Puede ser invocada desde Raspberry Pi mediante el comando: [10,<speed>,0] (bytes).

Parámetros

Recibe el parámetro:

  1. byte speed: Velocidad de retroceso dentro del rango [0,255].
Descripción de la función

Escala el valor recibido en el parámetro speed desde [0,255] hasta [0,KMAXSPEED] y establece con este valor la velocidad de retroceso del motor derecho.

Código fuente
void moveBackRight(byte speed)
{
	//Sets speed between 0 and KMAXSPEED
	setBackwardSpeedRight(map(speed,0,255,0,KMAXSPEED));
}

Funciones de lectura infrarrojos

void sendFrontAndMiddle()

Esta función implementa el comando 13.

Puede ser invocada desde Raspberry Pi mediante el comando: [13,0,0] (bytes).

Parámetros

No recibe parámetros.

Descripción de la función

Realiza la lectura de todos los sensores infrarrojos haciendo uso de las funciones auxiliares readFront() y readMiddle(), y envía los valores por comunicación serie USB.

Código fuente
void sendFrontAndMiddle()
{
	readFront();
	readMiddle();

	for(int i = 0; i < 12; i++)
	{
		Serial.print(frontSensorBool[i]);
	}
	
	for (int i = 0; i< 9; i++)
	{
		Serial.print(middleSensorValue[i]);
	}
	Serial.println();
}

void sendFront()

Esta función implementa el comando 14.

Puede ser invocada desde Raspberry Pi mediante el comando: [14,0,0] (bytes).

Parámetros

No recibe parámetros.

Descripción de la función

Realiza la lectura binaria de sensores frontales haciendo uso de la función auxiliar readFront y envía los valores analógicos por comunicación serie USB.

Código fuente
void sendFront()
{	
	readFront();

	for(int i = 0; i < 12; i++)
	{
		Serial.print(frontSensorBool[i]);
	}
	Serial.println();
	
}

void sendMiddle()

Esta función implementa el comando 15.

Puede ser invocada desde Raspberry Pi mediante el comando: [15,0,0] (bytes).

Parámetros

No recibe parámetros.

Descripción de la función

Realiza la lectura de sensores centrales haciendo uso de la función auxiliar readMiddle() y envía los valores analógicos por comunicación serie USB.

Código fuente
void sendMiddle()
{
	readMiddle();
	
	for (int i = 0; i< 9; i++)
	{
		Serial.print(middleSensorValue[i]);
	}
	Serial.println();
}

void sendAnalogFront()

Esta función implementa el comando 16.

Puede ser invocada desde Raspberry Pi mediante el comando: [16,0,0] (bytes).

Parámetros

No recibe parámetros.

Descripción de la función

Realiza la lectura analógica de los sensores frontales haciendo uso de la función auxiliar readFront() y envía los valores analógicos por comunicación serie USB.

Código fuente
void sendAnalogFront()
{
	readFront();
	
	for(int i = 0; i < 11; i++)
	{
		Serial.print(frontSensorValue[i]);
		Serial.print(",");
	}
	Serial.println(frontSensorValue[11]);
}

void readMiddle()

Función interna, es invocada desde otras funciones, nunca directamente desde la Raspberry Pi.

Parámetros

No recibe parámetros.

Descripción de la función

Realiza la lectura digital de los sensores centrales, almacenando su valor en la lista global middleSensorValue.

Los pasos que ejecuta el código son los siguientes.

  1. Enciende la matriz de sensores centrales.
  2. Para cada sensor, realiza su lectura binaria.
  3. Apaga la matriz de sensores centrales.
Código fuente
void readMiddle()
{
	digitalWrite(O_IRON_DG,HIGH);
	delay(10);	
	for ( int i = 0 ;  i < 9; i++)
	{
		middleSensorValue[i] = !digitalRead(MIDDLESENSORS[i]);
	}
	digitalWrite(O_IRON_DG,LOW);
}

void readFront()

Función interna, es invocada desde otras funciones, nunca directamente desde la Raspberry Pi.

Parámetros

No recibe parámetros.

Descripción de la función

Realiza la lectura de los sensores infrarrojos frontales y almacena sus valores en frontSensorValue (valores analógicos en el rango [0,2000]) y frontSensorBool (valores binarios).

La técnica empleada para la lectura analógica del sonar está basada en el cálculo del tiempo que tarda un sensor infrarrojo CNY70 en cargar un condensador de 10nF. Para ello, el código realiza los siguientes pasos:

  1. Carga todos los condensadores (12, uno por cada CNY70).
  2. Descarga todos los condensadores.
  3. Enciende la matriz de infrarrojos.
  4. Realiza una lectura no secuencial de los sensores haciendo uso de la variable auxiliar isSensorRead. Cuando un condensador se carga realiza un cortocircuito por lo que su lectura devuelve LOW. Por esto, se va realizando la lectura de los sensores. Cuando su lectura devuelve LOW significa que el condesador se ha cargado, por lo que se establece como valor de ese sensor el tiempo que ha tardado en cargarse (medido en ciclos del bucle). Si un condensador no se descarga en 2000 ciclos (valor de FRONTAL_TIMEOUT), se establece como 0. Además de almacenar el valor analógico en frontSensorValue, se almacena en binario en la variable frontSensorBool realizando una comparación con CNY70DIVIDER, cuyo valor es 30. Es decir, si el valor medido alcanza o supera los 30 ciclos, se establece como true (negro). En caso contrario, su valor booleano será false (blanco).
  5. Reestablece el vector isSensorRead a false.
  6. Apaga la matriz de sensores frontales.
Código fuente
void readFront()
{
	//Load capacitors
	for (int i = 0; i < 12; i++)
	{
		pinMode(FRONTSENSORS[i], OUTPUT);
		digitalWrite(FRONTSENSORS[i],HIGH);
	}
	delayMicroseconds(15);
	
	//Unload capacitors
	for (int i = 0; i < 12; i++)
	{
		pinMode(FRONTSENSORS[i], INPUT);
		digitalWrite(FRONTSENSORS[i],LOW);
	}
	
	//Switch on IR matrix
	digitalWrite(O_IRON_AN,HIGH);
	delayMicroseconds(5);
	
	//Read
	int cTiempo = 0;
	int cSensores = 0;
	while (cTiempo < FRONTAL_TIMEOUT && cSensores < 12)
	{
		for (int i = 0; i < 12; i++)
		{
			if (digitalRead(FRONTSENSORS[i]) == LOW && !isSensorRead[i])
			{
				frontSensorValue[i] = cTiempo;
				frontSensorBool[i] = cTiempo >= CNY70DIVIDER;
				
				isSensorRead[i] = true;
				cSensores++;
			}
		}
		cTiempo++;
		delayMicroseconds(1);
	}
	
	//Reset and set timeout values
	for (int i = 0; i < 12; i++)
	{
		if (isSensorRead[i])
		{
			isSensorRead[i] = false;
		}
		else
		{
			frontSensorValue[i] = 0;
			frontSensorBool[i] = 0;
		}
	}
	
	//Switch off IR matrix
	digitalWrite(O_IRON_AN,LOW);
}

Otros comandos

void led(byte num,bool status)

Esta función implementa el comando 12.

Puede ser invocada desde Raspberry Pi mediante el comando: [12,<numLed>,<state>] (bytes).

Parámetros

Recibe los parámetros:

  1. byte num: Número de led a controlar en el rango [0,13].
  2. bool status: Estado del led.
  • false o 0 indica estado apagado.
  • true u otro valor indica estado encendido.
Descripción de la función

Controla el estado de los leds. Esta función puede ser invocada por comunicación USB con el comando 12, y recibe 2 parámetros. El primero, entre 0 y 13 se corresponde con el número de led. El segundo indica el estado (1 es encendido, y 0 apagado).

La placa GoShield-GR lleva incorporados 14 leds, 12 de ellos en las posiciones de los sensores frontales, y otros 2 en el centro. Por tanto, para establecer su estado (encendido o apagado), se envía como primer parámetro un valor numérico entre 0 y 13, donde 0 es el led situado más a la izquierda, 11 el que está más a la derecha, y 13 y 14 los dos leds adicionales situados en el centro.

Esta función comprueba el rango de los leds, entre o y 13. Si el valor es mayor, encenderá el led número 13, correspondiente al led secundario central situado ligeramente a la derecha.

Por otra parte, recibe como segundo parámetro un booleano que indica el estado del led. Si un valor es distinto de 0, se considera true y se encenderá el led. En caso de recibir un 0, el estado del led será apagado.

Código fuente
void led(byte num,bool status)
{
	if		(num < 0)			num = 0;
	else if (num >= NUMLEDS)	num = NUMLEDS-1;
	
	digitalWrite(LEDS[num],status);
}

void beep(bool status)

Esta función implementa el comando 11.

Puede ser invocada desde Raspberry Pi mediante el comando: [11,<status>,0] (bytes).

Parámetros

Recibe el parámetro:

  1. bool status: Estado del zumbador.
  • false o 0 indica estado apagado.
  • true u otro valor indica estado encendido.
Descripción de la función

Establece el estado del zumbador, o claxon. 0 equivale a apagado, y cualquier otro valor (entre 1 y 255) establece el zumbador a activo.

Código fuente
void beep(bool status)
{
	digitalWrite(O_BUZZER,status);
}
You can’t perform that action at this time.
You signed in with another tab or window. Reload to refresh your session. You signed out in another tab or window. Reload to refresh your session.
Press h to open a hovercard with more details.