/***********************************
* Project: XMasRoBOT *
* Created by: Ricardo Dias (2009) *
***********************************/
/* BIBLIOTECAS NECESSARIAS */
#include <Servo.h>
/* VARIAVEIS DE CONFIGURACAO */
// Sensores
const int numSensors = 5;
const int TWO_SENSORS_EQUAL_DELTA = 25; // Diferenca maxima entre 2 sensores para os considerar iguais
const int sensor[] = {5,4,3,2,1}; // Array que tem os pinos dos sensores
// Servos
const int lServoPin = 9; // Pino do servo esquerdo (PWM)
const int rServoPin = 10; // Pino do servo direito (PWM)
const int MAX_SERVO_VEL = 7; // Velocidade maxima do Servo (entre 0 e 90)
const int MAX_TIME_WITHOUT_LINE_MS = 2000; // Tempo maximo sem linha (ou tempo de chegar ao centro do circulo)
Servo lServo; //
Servo rServo; //
/* VARIAVEIS AUXILIARES */
int senVal[] = {0,0,0,0,0}; // Array que tem os valores lidos pelos sensores
int targetVelL = 0; // Velocidade pretendida para o servomotor esquerdo
int targetVelR = 0; // Velocidade pretendida para o servomotor direito
int nowVelL = 0; // Velocidade actual do servomotor esquerdo
int nowVelR = 0; // Velocidade actual do servomotor direito
int cStart = 0; // Vai funcionar como timer
boolean stop = false;
double lineErrorVal = 0.0;
void setup(){
Serial.begin(9600);
pinMode(lServoPin, OUTPUT);
pinMode(rServoPin, OUTPUT);
lServo.attach(lServoPin);
rServo.attach(rServoPin);
Serial.println("ready!");
}
void loop(){
stop = false;
// Primeiro lemos os valores dos sensores
readAllSensors();
//Serial.println("readVals");
for(int i = 0; i < numSensors; i++){
Serial.print(senVal[i]);
Serial.print(" - ");
}
double average = sensorAverage(senVal);
if(average > 30){
stop = true;
}
// Vemos se os valores lidos sao todos iguais
if(stop){
cStart++;
if(cStart >= 17){
Serial.println("Start Point");
targetVelL = 0;
targetVelR = 0;
}
}else{
if(foundLine(senVal)){
lineErrorVal = lineError(senVal);
}
cStart = 0;
// Depois, determinamos as velocidades pretendidas para os motores
// Os motores irao ter uma velocidade entre 0 e "MAX_SERVO_VEL"
if(lineErrorVal < 0.0){
targetVelL = (int)(MAX_SERVO_VEL*(1-fabs(lineErrorVal)));
targetVelR = (int)(MAX_SERVO_VEL);
if(nowVelL <= 0.0)
targetVelL = -MAX_SERVO_VEL;
}else if(lineErrorVal > 0.0){
targetVelL = (int)(MAX_SERVO_VEL);
targetVelR = (int)(MAX_SERVO_VEL*(1-lineErrorVal));
if(nowVelR <= 0.0)
targetVelR = -MAX_SERVO_VEL;
}else{
targetVelL = (int)(MAX_SERVO_VEL);
targetVelR = (int)(MAX_SERVO_VEL);
}
}
// Atenuar os arranques e travagens
if(targetVelL > nowVelL){
nowVelL += 1;
}else if(targetVelL < nowVelL){
nowVelL -= 1;
}
if(targetVelR > nowVelR){
nowVelR += 1;
}else if(targetVelR < nowVelR){
nowVelR -= 1;
}
// cropping - ter a certeza que os valores se mantêm entre os pretendidos
if(nowVelL > MAX_SERVO_VEL){
nowVelL = MAX_SERVO_VEL;
}
if(nowVelR > MAX_SERVO_VEL){
nowVelR = MAX_SERVO_VEL;
}
// Fazer actuar os motores
lServo.write(90+nowVelL);
rServo.write(90-nowVelR);
delay(10);
}
//--- FUNCOES ---//
void readAllSensors(){
for(int i=0; i < 5; i++)
{
senVal[i] = analogRead(sensor[i]);
}
}
boolean foundLine(int senVal[])
{
boolean found = false;
double average = sensorAverage(senVal);
for(int i = 0; i < numSensors; i++){
if(senVal[i] - TWO_SENSORS_EQUAL_DELTA/4 > average)
found = true;
}
return found;
}
// Escolhe o sensor que esta mais proximo da linh
int bestSensor(int senVal[])
{
int best = 1;
int maxim = 0;
for(int i=0; i < 5; i++)
{
if(senVal[i] > maxim){
best = i+1;
maxim = senVal[i];
}
}
return best;
}
// Retorna um valor real entre -1.0 e 1.0, correspondente ao erro da posicao da linha
// Retorna 0.0 quando a linha esta ao centro
double lineError(int senVal[])
{
int best = bestSensor(senVal);
Serial.print(" :: BEST: ");
Serial.print(best);
Serial.print(" :: ");
double error = 0.0;
// Se o que tem a linha mais proxima esta na ponta, só podemos comparar com 1
if(best == 1){
error = -1.0;
if(fabs(senVal[0] - senVal[1]) <= TWO_SENSORS_EQUAL_DELTA){ error += 0.25; }
}else if(best == 2){
error = -0.5;
if(fabs(senVal[1] - senVal[0]) <= TWO_SENSORS_EQUAL_DELTA){ error -= 0.25; }
else if(fabs(senVal[1] - senVal[2]) <= TWO_SENSORS_EQUAL_DELTA){ error += 0.25; }
}else if(best == 3){
error = 0.0;
if(fabs(senVal[2] - senVal[1]) <= TWO_SENSORS_EQUAL_DELTA){ error -= 0.25; }
else if(fabs(senVal[2] - senVal[3]) <= TWO_SENSORS_EQUAL_DELTA){ error += 0.25; }
}else if(best == 4){
error = 0.5;
if(fabs(senVal[3] - senVal[2]) <= TWO_SENSORS_EQUAL_DELTA){ error -= 0.25; }
else if(fabs(senVal[3] - senVal[4]) <= TWO_SENSORS_EQUAL_DELTA){ error += 0.25; }
}else if(best == 5){
error = 1.0;
if(fabs(senVal[4] - senVal[3]) <= TWO_SENSORS_EQUAL_DELTA){ error -= 0.25; }
}
return error;
}
// Retorna a media dos valores de todos os sensores
double sensorAverage(int senVal[])
{
double avg = 0.0;
double soma = 0.0;
//int numSensors = sizeof(senVal);
for(int i=0; i < numSensors; i++){
soma += senVal[i];
}
avg = soma/numSensors;
return avg;
}
// Retorna TRUE se os sensores lerem todos aproximadamente o mesmo valor
boolean allEqual(int senVal[])
{
double average = sensorAverage(senVal);
boolean result = true;
for(int i=0; i < numSensors; i++){
if(fabs(senVal[i] - average) <= TWO_SENSORS_EQUAL_DELTA){
result = false;
break;
}
}
return result;
}
#include <Tone.h>
Tone tone1;
#define OCTAVE_OFFSET 0
int notes[] = { 0,
NOTE_C4, NOTE_CS4, NOTE_D4, NOTE_DS4, NOTE_E4, NOTE_F4, NOTE_FS4, NOTE_G4, NOTE_GS4, NOTE_A4, NOTE_AS4, NOTE_B4,
NOTE_C5, NOTE_CS5, NOTE_D5, NOTE_DS5, NOTE_E5, NOTE_F5, NOTE_FS5, NOTE_G5, NOTE_GS5, NOTE_A5, NOTE_AS5, NOTE_B5,
NOTE_C6, NOTE_CS6, NOTE_D6, NOTE_DS6, NOTE_E6, NOTE_F6, NOTE_FS6, NOTE_G6, NOTE_GS6, NOTE_A6, NOTE_AS6, NOTE_B6,
NOTE_C7, NOTE_CS7, NOTE_D7, NOTE_DS7, NOTE_E7, NOTE_F7, NOTE_FS7, NOTE_G7, NOTE_GS7, NOTE_A7, NOTE_AS7, NOTE_B7
};
char *song = "JingleBells:d=8,o=5,b=112:32p,a,a,4a,a,a,4a,a,c6,f.,16g,2a,a#,a#,a#.,16a#,a#,a,a.,16a,a,g,g,a,4g,4c6";
void setup(void)
{
Serial.begin(9600);
tone1.begin(11);
}
#define isdigit(n) (n >= '0' && n <= '9')
void play_rtttl(char *p)
{
// Absolutely no error checking in here
byte default_dur = 4;
byte default_oct = 6;
int bpm = 63;
int num;
long wholenote;
long duration;
byte note;
byte scale;
// format: d=N,o=N,b=NNN:
// find the start (skip name, etc)
while(*p != ':') p++; // ignore name
p++; // skip ':'
// get default duration
if(*p == 'd')
{
p++; p++; // skip "d="
num = 0;
while(isdigit(*p))
{
num = (num * 10) + (*p++ - '0');
}
if(num > 0) default_dur = num;
p++; // skip comma
}
Serial.print("ddur: "); Serial.println(default_dur, 10);
// get default octave
if(*p == 'o')
{
p++; p++; // skip "o="
num = *p++ - '0';
if(num >= 3 && num <=7) default_oct = num;
p++; // skip comma
}
Serial.print("doct: "); Serial.println(default_oct, 10);
// get BPM
if(*p == 'b')
{
p++; p++; // skip "b="
num = 0;
while(isdigit(*p))
{
num = (num * 10) + (*p++ - '0');
}
bpm = num;
p++; // skip colon
}
Serial.print("bpm: "); Serial.println(bpm, 10);
// BPM usually expresses the number of quarter notes per minute
wholenote = (60 * 1000L / bpm) * 4; // this is the time for whole note (in milliseconds)
Serial.print("wn: "); Serial.println(wholenote, 10);
// now begin note loop
while(*p)
{
// first, get note duration, if available
num = 0;
while(isdigit(*p))
{
num = (num * 10) + (*p++ - '0');
}
if(num) duration = wholenote / num;
else duration = wholenote / default_dur; // we will need to check if we are a dotted note after
// now get the note
note = 0;
switch(*p)
{
case 'c':
note = 1;
break;
case 'd':
note = 3;
break;
case 'e':
note = 5;
break;
case 'f':
note = 6;
break;
case 'g':
note = 8;
break;
case 'a':
note = 10;
break;
case 'b':
note = 12;
break;
case 'p':
default:
note = 0;
}
p++;
// now, get optional '#' sharp
if(*p == '#')
{
note++;
p++;
}
// now, get optional '.' dotted note
if(*p == '.')
{
duration += duration/2;
p++;
}
// now, get scale
if(isdigit(*p))
{
scale = *p - '0';
p++;
}
else
{
scale = default_oct;
}
scale += OCTAVE_OFFSET;
if(*p == ',')
p++; // skip comma for next note (or we may be at the end)
// now play the note
if(note)
{
Serial.print("Playing: ");
Serial.print(scale, 10); Serial.print(' ');
Serial.print(note, 10); Serial.print(" (");
Serial.print(notes[(scale - 4) * 12 + note], 10);
Serial.print(") ");
Serial.println(duration, 10);
tone1.play(notes[(scale - 4) * 12 + note]);
delay(duration);
tone1.stop();
}
else
{
Serial.print("Pausing: ");
Serial.println(duration, 10);
delay(duration);
}
}
}
void loop(void)
{
play_rtttl(song);
Serial.println("Done.");
delay(100);
}
(https://lusorobotica.com/proxy.php?request=http%3A%2F%2Flh6.ggpht.com%2F_inn_7Qkw9v8%2FS0J8YJwLO7I%2FAAAAAAAABk8%2F67hjIRqZwjE%2Fs288%2FDSC04901.JPG&hash=2dcfb019ab3134ccbeb9dcd1caa3c90fbbc50035) O array de sensores | (https://lusorobotica.com/proxy.php?request=http%3A%2F%2Flh6.ggpht.com%2F_inn_7Qkw9v8%2FS0J8UENzLXI%2FAAAAAAAABko%2F1TLfrNk-gLE%2Fs288%2FDSC04886.JPG&hash=91e08e8aea1ba2c8325b9b54838881469cface47) A placa de controlo, ainda inacabada |
(https://lusorobotica.com/proxy.php?request=http%3A%2F%2Flh3.ggpht.com%2F_inn_7Qkw9v8%2FS0J8R50ZXwI%2FAAAAAAAABkY%2F3b16YLZtIxo%2Fs288%2FPicture%2520018.jpg&hash=6645afc7d955d482a683148bd1af2a3a29318336) As engrenagens metálicas dos servos. | (https://lusorobotica.com/proxy.php?request=http%3A%2F%2Flh6.ggpht.com%2F_inn_7Qkw9v8%2FS0J8WiuJvwI%2FAAAAAAAABk0%2Fe9aBo7oTP8o%2Fs288%2FDSC04891.JPG&hash=0b7d68843d5b79df8c65e6dfb3e36bc2ad060b3f) A chapa a implorar para ser cortada e dobrada xD |
(https://lusorobotica.com/proxy.php?request=http%3A%2F%2Flh3.ggpht.com%2F_inn_7Qkw9v8%2FS0J8bD6soAI%2FAAAAAAAABlQ%2FnxBAMUPNwGg%2Fs288%2FDSC04917.JPG&hash=5bcb25ce3b576a4f50ec31d4a6a0693d5ec0e61f) A chapa já cortada, dobrada e com a roda de apoio | (https://lusorobotica.com/proxy.php?request=http%3A%2F%2Flh5.ggpht.com%2F_inn_7Qkw9v8%2FS0J8c9nrWBI%2FAAAAAAAABlc%2FXekSOGhCnmc%2Fs288%2FDSC04920.JPG&hash=4d5bdc06a755e54f65e3af9416da3e9dd43e59a5) Os suportes para os servos na base |
(https://lusorobotica.com/proxy.php?request=http%3A%2F%2Flh3.ggpht.com%2F_inn_7Qkw9v8%2FS0J8eZCbdRI%2FAAAAAAAABlk%2FePrIsg4UaDE%2Fs288%2FDSC04925.JPG&hash=cefdaec2ba977909ce45dd56706bda46fa666eaa) Já começa a ganhar forma :) | (https://lusorobotica.com/proxy.php?request=http%3A%2F%2Flh3.ggpht.com%2F_inn_7Qkw9v8%2FS0J8gnYqDSI%2FAAAAAAAABlw%2FfgpuPK2QlXU%2Fs288%2FDSC04931.JPG&hash=91e2229f1ddf1ae6d8f3e2ee6d0611221c4c9706) A estudar o apoio para as prendas... |
(https://lusorobotica.com/proxy.php?request=http%3A%2F%2Flh6.ggpht.com%2F_inn_7Qkw9v8%2FS0J8e_sEqtI%2FAAAAAAAABlo%2FLnuN4hIO8Ho%2Fs288%2FDSC04926.JPG&hash=c577324f0d05bea35f5b8b2ccd47dd8ee18b7b06) A pista já montada | (https://lusorobotica.com/proxy.php?request=http%3A%2F%2Flh4.ggpht.com%2F_inn_7Qkw9v8%2FS0J8iz82YPI%2FAAAAAAAABmA%2Fj1U-mSxAtYY%2Fs288%2FDSC04935.JPG&hash=2b387dd4e6327bd2ca472a50b30bfc34061b9b82) O robô final e o pai natal pronto para dar umas voltas |
...vi que a biblioteca Tone usa os timers todos do Arduino e a biblioteca dos servos necessita do timer 2 (se não estou em erro). Ainda andei a tentar alterar o Tone library para ver se conseguia torná-los compatíveis, mas andar a mexer nos timers fez com que a música ficasse toda maluca, sem tempo.
Solução: 2 arduinos - um para a música e outro para as decisões (sensores + servos).
Muito bom, mas olha as regras das prendas enquanto tens tempo ;)Referes-te ao tamanho? É no mínimo 10x10x10, ela tem a base 10.5x10.5 (foto (http://img97.imageshack.us/img97/5882/dsc04944q.jpg)) e altura cerca de 23cm (foto (http://img30.imageshack.us/img30/5240/dsc04945y.jpg))...
3. O desafio consiste na criação de um robot capaz de transportar no mínimo uma prenda, cada prenda deve ter as seguintes dimensões mínimas:
a. Altura: 10cm
b. Comprimento: 10cm
c. Largura: 10cm
:D ficou muito bom! acho ser dos mehlores até agora, senão o melhor!
tiveste problemas com alfandega e a deal extreme?
Objectivos
O objectivo é ajudar o Pai Natal daqui a 12 meses ;D
Objectivos
O objectivo é ajudar o Pai Natal daqui a 12 meses ;D
O Pai Natal hoje ainda anda a entregar as prendas em Espanha :D