Alessandro Crono
Robot Octagon
Tutorials -
Giovedì 24 Febbraio 2011 21:12
Scritto da Alessandro Crono

 

 

 

Ciao ragazzi finalmente sono riuscito a fare il tutorials del robottino Octagon. Spero via sia utile :D

 

 

ECCO I VIDEO DEL ROBOT :

 

1 - http://www.youtube.com/watch?v=gBSJ8aBEEi0

2 - http://www.youtube.com/watch?v=9ToZNYYD8xM

 

Occorrente:


 

1 x Octagon Livello 1 http://www.gioblu.com/prodotti?page=shop.product_details&flypage=flypage_images.tpl&product_id=136&category_id=60

1 x Octagon Livello 2 http://www.gioblu.com/prodotti?page=shop.product_details&flypage=flypage_images.tpl&product_id=135&category_id=60

1 x Octagon Livello 3 http://www.gioblu.com/prodotti?page=shop.product_details&flypage=flypage_images.tpl&product_id=133&category_id=60

1 x Piastra Pico http://www.gioblu.com/prodotti?page=shop.product_details&flypage=flypage_images.tpl&product_id=132&category_id=60

1 x ARDUINO DUEMILANOVE

2 x Staffe per Servi http://www.gioblu.com/prodotti?page=shop.product_details&flypage=flypage.tpl&product_id=78&category_id=34

1 x Staffa microservo http://www.gioblu.com/prodotti?page=shop.product_details&flypage=flypage.tpl&product_id=158&category_id=34

2 x Servi Standard http://www.gioblu.com/prodotti?page=shop.product_details&flypage=flypage_images.tpl&product_id=141&category_id=56

1 x Microservo http://www.gioblu.com/prodotti?page=shop.product_details&flypage=flypage_images.tpl&product_id=138&category_id=56

2 x Ruote http://www.gioblu.com/prodotti?page=shop.product_details&flypage=flypage_images.tpl&product_id=134&category_id=61

1 x Ruota pivontante http://www.gioblu.com/prodotti?page=shop.product_details&flypage=flypage_images.tpl&product_id=128&category_id=59

1 x Range Sensor URM37

1 x fotoresistenza

 

Montaggio:


Per prima cosa bisogna procedere seguendo questo tutorials: http://www.gioblu.com/tutorials/azionamenti/94-come-modificare-un-servomotore che permetterà di trasformare dei servi standard in servi a rotazione continua. Fatto questo montiamo la staffetta dei servi e tra staffetta e ruota mettiamo un paio di rondelle per poter avere la ruota perfettamente dritta, ora montiamo i servi alle staffette per servi e le montiamo sul Octagon livello 1 seguendo questo tutorials: http://www.gioblu.com/tutorials/prodotti-gioblu/140-octagon-livello-1 a sua volta montiamo la ruota pivontante che ci permette di tenere il robot in equilibrio. Io ho deciso di mettere il microservo per il range sensor fermato con una barra filettata e sopra gli ho messo il pico che lo ferma a panino. Con dei prigioneri o sempre una barra filettata ho montato il secondo livello octagon poi ho montato la staffetta del microservo con il range sensor. Il terzo livello serve solo per rifinire con un po di estetica il robot. Ora il robot è montato senza nessun problema.

P.S Consiglio di mettere delle rondelle tra le staffe dei servi e il primo livello octagon perchè se no la ruota con pneumatico gratta contro la plastica e si rovina il pneumatico, se non basta mettere un dado.

 

Cablaggio:


Il sensore urm37 ho usato questi pin del arduino:

Collegati in TTL

  • 5v (Arduino) --->Pin 1 VCC (URM V3.2)

  • GND (Arduino) ---->Pin 2 GND (URM V3.2)

  • Pin11 (Arduino) ---->Pin 7 (URM V3.2)

  • Pin 7 (Arduino) ---->Pin 8 (URM V3.2)

  • Pin 6 (Arduino) ---->Pin 9 (URM V3.2)

I servi

  • servo destro pin 9

  • servo sinistro pin 10

Microservo pin 3

 

Fotoresistenza:

           PhotoR     10K
+5 o---/\/\/--.--/\/\/---o GND
|
Pin o-----------

CODICE:


Questo codice lavora in due modi ho collegato via seriale al Pc cioè lo si può comandare dalla tastiera del pc oppure se viene premuto il push button parte in modalità automatica cioè lavora il Range Sensor, ha un controllo della luce ambientale per far accendere dei led ad alta luminosità quando il posto è troppo buio..Usando dei moduli WPM è possibile comandare il pc in seriale dalla tastiera del pc con due moduli, senza usare il cavo che è piuttosto scomodo.. ecco il tutorials dei moduli wpm: http://www.gioblu.com/tutorials/comunicazione/197-wireless-programming-module-for-arduino

P.S INCLUDERE LIBRERIA SERVO

#include
#include "URMSerial.h"

Servo sservo; // Servo sinistro
Servo dservo; // Servo destro
Servo uservo; // Servo centrale Range Scanner
int ang=0;
int angolomax=0;
int distmax=0;
int value=100;
int ind=0;
int rotazione=0;

// Definizione delle misure
#define DISTANCE 1
#define TEMPERATURE 2
#define ERROR 3
#define NOTREADY 4

//angolo di riposo dei servi delle ruote
#define RIPOSOS 90
#define RIPOSOD 90

// inizializza l'oggetto della libreria urm del sensore ultrasuoni
URMSerial urm;
const int  buttonPin = 2;    // the pin that the pushbutton is attached to
int buttonPushCounter = 0;   // counter for the number of button presses
int buttonState = 0;         // current state of the button
int lastButtonState = 0;     // previous state of the button


void setup()
{
Serial.begin(57600);  // Imposta il baud rate a 9600
urm.begin(6,7,9600); // RX Pin, TX Pin, Baud Rate
pinMode(13, OUTPUT);
pinMode(buttonPin, INPUT);
sservo.attach(10);   // Attacca il servo sinistro al pin 10
dservo.attach(9);    // Attacca il servo destro al pin 9
uservo.attach(3);    // Attacca il servo degli ultrasuoni al pin 3
uservo.write(90);    // metti il servo degli ultrasuoni dritto
sservo.write(RIPOSOS); // ferma il robot
dservo.write(RIPOSOD); // ferma il robo
delay(200);
}

void loop()
{

int val;
val=analogRead(0);   //Sensore Termoresistenza
Serial.print("Quantità Luce ");
Serial.println(val,DEC);//Stampa Valore Termoresistenza  
delay(100);
if ( val > 890)
{
digitalWrite(13, HIGH);
}
else if ( val < 600)
{
digitalWrite(13, LOW);
}
buttonState = digitalRead(buttonPin);

// buttonState è diverso di lastButtonState
if (buttonState != lastButtonState) {
// se buttonState e on
if (buttonState == HIGH) {
//conta qualche volte
buttonPushCounter++;
}
}
//lastButtonState è uguale a buttonState
lastButtonState = buttonState;


// se buttonPushCounter è = a 0
if (buttonPushCounter == 0){
tasti();
}

if(buttonPushCounter == 1){
sservo.write(RIPOSOS); // ferma il robot
dservo.write(RIPOSOD); // ferma il robot
sservo.write(RIPOSOS + 90); // fai andare il robot dritto
dservo.write(RIPOSOD - 90); // fai andare il robot dritto

// Richiedi la distanza al sensore ad ultrasuoni
urm.requestMeasurement(DISTANCE);
delay(50); // attendi allineamento dei servi
if(urm.hasReading())
{
switch(urm.getMeasurement(value)) // Verifica che il valore letto sia una distanza
{
case DISTANCE: // Ulteriore verifica che sia una distanza

if (value>0 & value<30)  // se il valore e' < 20 cm stiamo per sbatter
{
angolomax=0; // imposta l'angolo di migliore uscita a zero
distmax=0; // imposta la distanza di uscita migliore a zero
uservo.write(5);  // sposta il servo degli ultrasuoni a destra
sservo.write(RIPOSOS);  // ferma il servo in posizione di riposo
dservo.write(RIPOSOD);  // ferma il servo in posizione di riposo
for (ang=0; ang<=9; ang++) // fai un ciclo per controllare dove sono gli ostacoli
{
uservo.write(ang*20); // imposta il servo degli ultrasuoni sul valore assunto da ang
urm.requestMeasurement(DISTANCE); //richiedi una nuova lettura della distanza
delay(50); //attendi allineamento servo ultrasuoni
if(urm.hasReading()) // verifica se ha letto
{
switch(urm.getMeasurement(value)) // Verifica che il valore letto sia una distanza
{
case DISTANCE: // Ulteriore verifica che sia una distanza
if (value > distmax) // verifica che la distanza letta sia maggiore del max
{
angolomax=ang*20;  // se e' maggiore del max imposta il nuovo angolo max e
distmax=value;  // la nuova distanza max
} //end if (value > distmax)
break;                           
} // end switch interno
} // end if(urm.hasReading())
} //end for (ang=0; ang<180; ang +=5)        

uservo.write(90); //rimetti il servo degli ultrasuoni dritto
// scrivi sulla seriale il max valore e il max angolo rilevati
Serial.print("Angolo max \t");
Serial.println(angolomax);

//imposta i servi sull'angolo di max distanza per non toccare l'ostacolo

if (angolomax > 180)
angolomax=180;
if (angolomax < 0)
angolomax=0;

rotazione=abs(angolomax-90); //serve per far durare di piu la rotazione

// scrivi un po' di valori di controllo nel serial monitor
Serial.print("Angolo inviato ai servi \t");
Serial.println(RIPOSOS+angolomax-90);
Serial.print("Valore di rotazione \t");
Serial.println(rotazione / 5);

// fai andare i servi verso l'angolo di max distanza
for (ind=1; ind < rotazione/5; ind+=1)
{
sservo.write(RIPOSOS+angolomax-90);
dservo.write(RIPOSOD+angolomax-90);
delay (40); // attendi allineamento servi
} // end for (ind=1; ind < rotazione / 9; ind+=1)
} //  end if (value 0)
break;   
} // end switch esterno
} // end if(urm.hasReading()) esterno
} // end else Robot Seriale
} //end loop

char a;
long previousMillis = 0;      
long interval = 600;
long intervall = 400;

void tasti()
{

if(Serial.available() > 0){
a = Serial.read();
sservo.write(RIPOSOS); // ferma il robot
dservo.write(RIPOSOD); // ferma il robot
switch(a){
case 'w':
previousMillis = millis();  
while(millis() - previousMillis < interval ) {
Serial.println("avanti");
for( int throttle = 0.0001; throttle < 91; throttle++) {
sservo.write(RIPOSOS + 90 + throttle); // fai andare il robot dritto
dservo.write(RIPOSOD - 90 + throttle); // fai andare il robot dritto
}
}
sservo.write(RIPOSOS); // ferma il robot
dservo.write(RIPOSOD); // ferma il robot
break;
case 's':
previousMillis = millis();  
while(millis() - previousMillis < interval) {
Serial.println("indietro");
sservo.write(RIPOSOS - 90); // fai andare il robot indietro
dservo.write(RIPOSOD + 90); // fai andare il robot indietro
}
sservo.write(RIPOSOS); // ferma il robot
dservo.write(RIPOSOD); // ferma il robot
break;
case 'd':
previousMillis = millis();  
while(millis() - previousMillis < interval - intervall) {
Serial.println("destra");
sservo.write(RIPOSOS - 90); // fai andare il robot a destra
dservo.write(RIPOSOD - 90); // fai andare il robot a destra
}
sservo.write(RIPOSOS); // ferma il robot
dservo.write(RIPOSOD); // ferma il robot
break;
case 'a':
previousMillis = millis();  
while(millis() - previousMillis < interval - intervall) {
Serial.println("sinistra");
sservo.write(RIPOSOS + 90); // fai andare il robot a sinistra
dservo.write(RIPOSOD + 90); // fai andare il robot a sinistra
}
sservo.write(RIPOSOS); // ferma il robot
dservo.write(RIPOSOD); // ferma il robot
break;
case 'c':
previousMillis = millis();  
while(a != 'v') {
//Serial.println("avanti tutta");
a = Serial.read();
sservo.write(RIPOSOS + 90); // fai andare il robot avanti continuo
dservo.write(RIPOSOD - 90); // fai andare il robot avanti continuo
}
sservo.write(RIPOSOS); // ferma il robot
dservo.write(RIPOSOD); // ferma il robot
break;
}
}
}

 

Questa parte di codice:

 

for( int throttle = 0.0001; throttle < 91; throttle++)

 

serve per far partire piu lentamente il servo per evitare che il robot si impenna quando parte!!

La funzione Void Tasti (); serve per usare il robot in seriale e comandarlo dalla tastiera del pc

 

BUON DIVERTIMENTO!!

 

Gioblu Robotics ¬© 2010 - 2012 · Sitemap · privacy

gioscarab@gmail.com

Gioblu BOTServer è online dal 10 Aprile 2010 - 319.232 Visite - 1.027.175 Pagine visualizzate - 182.309 Visitatori unici - 536 utenti attivi