Pasar de script a Programacion Orientado a Objeto

Hola, primero de todo, saludos. Soy nuevo en el foro, y estoy dando mis primeros pasos con Arduino. Actualmente me dedico a la programacion de web. Por desgracia no tengo mucha idea de programacion C.

Os cuento, estoy haciendo un robot, tengo un sketch que lo hace funcionar, pero al añadir todo el codigo en el sketch, se me empieza a liar la cosa, por ello me he decidido a intentar orientarlo a objeto y crear varias clases que me estructuren el codigo y pueda asi modificarlo y mantenerlo con mas claridad.

Para simplificar he creado una clase para dirigir los dos servos que componen la “torreta”, sobre la cual va el sensor de ultrasonidos, que permitira “ver” el entorno al robot. Esta “torreta”, no son mas que dos servos. Uno puesto en vertical, que mueve una plataforma horizontalmente, hacia los lados. En la plataforma he atornillado otro servo, en horizontal, para que mueva el sensor de ultrasonidos verticalmente, hacia ariba y abajo. De manera que moviendo los servos pueda apuntar el sensor de ultrasonidos hacia cualquier punto y tomar medidas.

La clase creada es:

Archivo “Tower.h”

#ifndef Tower_H
#define Tower_H

#include “Arduino.h”
#include <Servo.h>

#define TOWER_VERSION 1

class Tower {
public:
Tower(byte pinServHorz, byte pinServVert);
~Tower();
void begin();
void moveTo(byte x, byte y, boolean hasDelay = true);
void lookFront();
void lookBack();
void lookUnder();
void lookUnderFront();
void lookUnderBack();
private:
Servo servHorz;
Servo servVert;
};

#endif

Archivo “Tower.cpp”

#include “Tower.h”

Tower::Tower(byte pinServHorz, byte pinServVert) {

servHorz.attach(pinServHorz);
servVert.attach(pinServVert);

begin();
}

Tower::~Tower(){
/nothing to destruct/
}

void Tower::begin() {
/nothing to begin/
}

// x es c2 => horz
// y es c1 => vert
void Tower::moveTo(byte x, byte y, boolean hasDelay) {

//Ajust
y += 30; //from 30º to 120º
x += 4; //from 4º to 184º

byte yInt = servVert.read();
byte xInt = servHorz.read();

byte diference = max(abs(x – xInt), abs(y – yInt));

servVert.write(y);
servHorz.write(x);

Serial.print(“pto (”);
Serial.print(x);
Serial.print(", “);
Serial.print(y);
Serial.println(”)");

if (hasDelay)
delay(diference * 4);
}

void Tower::lookFront() {
moveTo(0,90,true);
}

void Tower::lookBack() {
moveTo(180,90,true);
}

void Tower::lookUnder() {
moveTo(90,0,true);
}

void Tower::lookUnderFront() {
moveTo(0,0,true);
}

void Tower::lookUnderBack() {
moveTo(180,0,true);
}

Archivo “tower_example.ino”

//Pin PWM servo Horizontal. Es el pin 13, ya que Arduino Leonardo tambien tiene pwm en dicho pin.
const byte TOWER_PINSERVHORZ = 13;
//Pin PWM servo Vertical
const byte TOWER_PINSERVVERT = 3;

Tower tower(TOWER_PINSERVHORZ, TOWER_PINSERVVERT);

void setup() {

Serial.begin(9600);

}

void loop() {

if ( Serial.available()) {
char ch = Serial.read();

switch(ch) {
case ‘w’:
//Mirar al frente
tower.lookFront();
Serial.println(“tower in front”);
break;
case ‘x’:
//Mirar hacia detras
tower.lookBack();
Serial.println(“tower in back”);
break;
case ‘s’:
//Mirar hacia abajo
tower.lookUnder();
Serial.println(“tower in under”);
break;
case ‘a’:
//Mirar hacia abajo
tower.lookUnderFront();
Serial.println(“tower in under front”);
break;
case ‘d’:
//Mirar hacia abajo
tower.lookUnderBack();
Serial.println(“tower in under back”);
break;
} //End switch ch

} //End if serial available

}

Al ejecutar esta clase en un Arduino Leonardo, los servos se comportan de una extraña forma, temblando y haciendo mucho ruido. Asi que he pensado que hay algo en el codigo mal escrito. Se me ocurrio crear el mismo codigo pero en formato script, para compararlos, y me funciona perfectamente.

Archivo “tower_simple_example.ino”

#include <Servo.h>

//servo ultrasonidos arriba-abajo
byte pinServVert = 3;
//servo ultrasonidos rotación
byte pinServHorz = 13;

Servo servVert; //sensor arriba-abajo
Servo servHorz; //sensor rotacion

// x es horz
// y es vert
void moveSensorTo(byte x, byte y, boolean hasDelay = true) {

//Ajust
y += 30; //from 30º to 120º
x += 4; //from 4º to 184º

byte yInt = servVert.read();
byte xInt = servHorz.read();

byte diference = max(abs(x – xInt), abs(y – yInt));

servVert.write(y);
servHorz.write(x);

Serial.print(“pto (”);
Serial.print(x);
Serial.print(", “);
Serial.print(y);
Serial.println(”)");

if (hasDelay)
delay(diference * 4);
}

void lookFront(boolean hasDelay = true) {
moveSensorTo(0,90);
}

void lookBack(boolean hasDelay = true) {
moveSensorTo(180,90);
}

void lookUnder(boolean hasDelay = true) {
moveSensorTo(90,0);
}

void lookUnderFront() {
moveSensorTo(0,0);
}

void lookUnderBack() {
moveSensorTo(180,0);
}

void setup() {
servVert.attach(pinServVert);
servHorz.attach(pinServHorz);

Serial.begin(9600);

}

void loop() {

if ( Serial.available()) {
char ch = Serial.read();

switch(ch) {
case ‘w’:
//Mirar al frente
lookFront();
Serial.println(“tower in front”);
break;
case ‘x’:
//Mirar hacia detras
lookBack();
Serial.println(“tower in back”);
break;
case ‘s’:
//Mirar hacia abajo
lookUnder();
Serial.println(“tower in under”);
break;
case ‘a’:
//Mirar hacia abajo
lookUnderFront();
Serial.println(“tower in under front”);
break;
case ‘d’:
//Mirar hacia abajo
lookUnderBack();
Serial.println(“tower in under back”);
break;
} //End switch ch

} //End if serial available

}

Estoy utilizando el pin 13, ya que tengo los demas pwm en uso. Arduino Leonardo tiene un pin mas de pwm en el 13. Pero no estoy seguro si eso crea un problema de algun tipo. El 13, en arduino se utiliza para otras cosas. no??

Tambien he probado cambiando el pin pero he obtenido los mismos resultados.

Le he estado dando vueltas y haciendo pruebas, llegando a un punto que no se que hacer. Se que seguramente, se me escape algo que todavia no se, y no doy con ello, por eso os pido vuestra ayuda.

Muchas gracias por vuestro tiempo.