PitMotor AutoPilot
PitMotor AutoPilot è l'annunciata evoluzione del PitMotor. E utilizza la scheda PitMotor assieme alla nuova scheda PitTesta.
La scheda attraverso l'integrato L293D (ponte H) controlla due motori indipendenti della macchinina sulla scheda viene inoltre montato un Arduino mini ....
La PitMotor attraverso l'integrato L293D (ponte H) controlla due motori indipendenti della macchinina sulla scheda doveva essere montato un Arduino mini che purtroppo non funziona più e quindi il tutto è stato ricablato con Arduino Uno.
La scheda PitMotor serve per controllare un piccolo rover con due ruote indipendenti e avere la possibilità di utilizzare gli altri pin di Arduino per i controlli (posizione, rotazione testa e comandi) .
Ai lati sono presenti anche due ponticelli da utilizzare per attivare o meno i motori. Se on il motore è attivo. Questi sono collegati ai pin 4 e 2 di Arduino. Se necessitano ulteriori pin per altri sviluppi. Si possono liberare i pin (cambiare i ponticelli) e alimentare direttamente il ponte H con VCC.
La scheda PitTesta serve per montare il sensore a Ultrasuoni sul Servomotore e integra i contatti di quest'ultimo e il sensore a infrarossi per i comandi. Tutti i collegamenti sono concetrati in un unico pin Header dal quale partono i cavi di connessione ad Arduino.
Il codice nella nuova versione è stato integrato con la parte di controllo della distanza. In pratica se il flag AutoPilot è attivo viene effettuata in continuo la misura della distanza. Quando questa è inferiore al valore di soglia il rover si ferma. La testa si muove su un angolo prestabilito e determina le relative distanze in ogni direzione.
Attaverso un loop si determina quale è la direzione da prendere. Il Rover effettua una rotazione, si riferma e riparte in automatico.
Nel caso del Rover in esame uno dei problemi è stato quello di riuscire a farlo andare diritto. I due motori sono infatti fortemente sbilanciati. E non si è ancora capito perchè. Comunque attualmente funziona abbastanza bene con ostacoli di grande dimensione. Per diminuire la distanza occorre affinare il software di controllo .. qualcuno suggerisce di montare più sensori di distanza. Ma questo è un'altro progetto.
Cosa serve:
- IC L293D (ponte H)
- Arduino Mini
- 1838B IR ricevitore infrarossi
- HC-SR04 - sensore di distanza ad ultrasuoni
- Telecomando di apparecchiatura rotta
- stagno
- resistenze
- condensatore
- pin header
Download:
PitMotor:
- PitMotor Schema PDF - top
- PitMotor Schema PDF - bottom
- PitMotor Schema Fritzing
- PitMotor Sketch Arduino
PitMotor AutoPilot:
- PitTesta Schema PDF - top
- PitTesta Schema PDF - bottom
- PitTesta Schema Fritzing
- PitMotor AutoPilot Sketch Arduino
Codice sorgente:
/*
PitMotor05.ino - pilota automatico - indroduzione di distanza di sicurezza
quando sotto distanza controllo con servo e decisione direzione
rotazione rover e ripartenza
attraverso telecomando possibile controllo
sia manuale che automatico
febbraio 2015
PitMotor04.ino - aggiunta di testa con sensore ultrasuoni e servomotore il controllo
dei componenti è manuale tramite telecomando
febbraio 2015
PitMotor03.ino - Sketch di controllo di una Rover con due motori
ZappocoS dicembre 2014 - gennaio 2015
PitMotor08 --> shield di controllo di due motori tramite Arduino Pro Mini
Modifica e sviluppo dello:
Sketch per il controllo bidirezionale della velocità di
due motori DC collegati a un driver L293.
Pier Calderan - Luglio 2013
*/
/* libreria per IR */
#include "IRremote.h"
#include <Servo.h>
// ------------------------------------------------------------------
// definizione dei pin di controllo
// ------------------------------------------------------------------
const int servoPin = 10; // pin del servomotore
const int receiver = 11; // pin segnale del ricevitore IR a Arduino digital pin ~5 (PWM)
IRrecv irrecv(receiver); // create instance of 'irrecv'
decode_results results; // create instance of 'decode_results'
// ------------------------------------------------------------------
// motore 1
// ------------------------------------------------------------------
const int enable1Pin = 2; // 7; // porta digitale per il pin 1 (1,2EN)
const int motor1Pin = 8; // 8; // porta digitale per il pin 2 (1A) --> direzione della rotazione
const int motor2Pin = 9; // 9; // porta digitale (PWM) per il pin 7 (2A) --> velocità della rotazione
// ------------------------------------------------------------------
// motore 2
// ------------------------------------------------------------------
const int enable2Pin = 4; // 4; // porta digitale per il pin 9 (3,4EN)
const int motor3Pin = 7; // 2; // porta digitale per il pin 10 (3A) --> direzione della rotazione
const int motor4Pin = 6; // 5; // porta digitale (PWM) per il pin 15 (4A) --> velocità della rotazione
// ------------------------------------------------------------------
// ultrasuoni
// ------------------------------------------------------------------
const int trigPin = 12; // pin del trigPin
const int echoPin = 13; // pin del echoPin
// ------------------------------------------------------------------
// variabili controllo motori
// ------------------------------------------------------------------
int enableMotor1 = HIGH;
int enableMotor2 = HIGH;
int direction1 = HIGH;
int direction2 = HIGH;
int deltaSpeed1 = 40;
int deltaSpeed2 = 100;
int speed1 = 2 * deltaSpeed1;
int speed2 = 2 * deltaSpeed2;
int spuntoFlag = false;
const int spuntoDelay = 20; // durata dello spunto
// ------------------------------------------------------------------
// variabili controllo distanze
// ------------------------------------------------------------------
long duration, cm;
int minDist = 0;
int maxDist = 0;
int minAngle = 0;
int maxAngle = 0;
// ------------------------------------------------------------------
// variabili pilota automatico
// ------------------------------------------------------------------
boolean autoPilotFlag = true; // flag controllo pilota automatico
int secureDist = 50; // distanza di sicurezza
// ------------------------------------------------------------------
// variabili controllo servomotore
// ------------------------------------------------------------------
int servoPos = 90; // angolo del servomotore
int servoMin = 60; // angolo del servomotore
int servoMax = 120; // angolo del servomotore
int servoDelta = 3; // angolo del servomotore
// ------------------------------------------------------------------
// controllo stampa seriale
// ------------------------------------------------------------------
boolean flagDebug = false;
Servo myservo; // create servo object to control a servo
void setup() {
Serial.begin(9600);
Serial.println("PitMotor V05 - by Zappoco");
Serial.println("pilota automatico 02/2015");
Serial.println("controllo distanze 02/2015");
Serial.println("manovra tramite IR 12/2014");
Serial.println("--------------------------");
Serial.println("");
// Ultrasuoni
pinMode(trigPin,OUTPUT);
pinMode(echoPin,INPUT);
// motore 1
pinMode(enable1Pin, OUTPUT);
pinMode(motor1Pin, OUTPUT);
pinMode(motor2Pin, OUTPUT);
// motore 2
pinMode(enable2Pin, OUTPUT);
pinMode(motor3Pin, OUTPUT);
pinMode(motor4Pin, OUTPUT);
// abilitazione dei due motori:
// digitalWrite(enable1Pin, HIGH); // --> per liberare un pin puo essere alimentato direttamente 5V
// digitalWrite(enable2Pin, HIGH); // --> per liberare un pin puo essere alimentato direttamente 5V
irrecv.enableIRIn(); // fa partire il ricevitore infrarossi
myservo.attach(servoPin); // attacca il servomotore al pin servoPin to the servo object
}
void loop() {
// ------------------------------------------------------------------
// controllo pilota automatico
// ------------------------------------------------------------------
if (autoPilotFlag && enableMotor1 && enableMotor2 ) {
// accende i motori
//digitalWrite(enable1Pin, HIGH); // accende motore 1
//digitalWrite(enable2Pin, HIGH); // accende motore 2
// controlla la distanza
PitTestaDistanza ();
if (cm <= secureDist)
{
// blocca il rover
speed1 = 0;
speed2 = 0;
PitMotor ();
// controlla la direzione da prendere
Serial.print(" controllo direzione da prendere ");
Serial.println();
PitRoverDirection ();
delay (1000);
// in funzione della direzione ruota il rover
if (maxAngle >= 90) { //destra
Serial.print(" rotazione a destra ");
Serial.println();
speed1 = 5 * deltaSpeed1;
speed2 = 5 * deltaSpeed2;
direction1 = LOW;
direction2 = HIGH;
spuntoFlag = false;
PitMotor ();
delay (800);
}
else{ //sinsitra
Serial.print(" rotazione a sinsitra ");
Serial.println();
speed1 = 5 * deltaSpeed1;
speed2 = 5 * deltaSpeed2;
direction1 = HIGH;
direction2 = LOW;
spuntoFlag = false;
PitMotor ();
delay (400);
}
// riparte in avanti
Serial.print(" rotazione ok - stop");
Serial.println();
speed1 = 0;
speed2 = 0;
PitMotor ();
delay (2000);
Serial.print(" riparte in avanti ");
Serial.println();
direction1 = HIGH;
direction2 = HIGH;
speed1 = 2 * deltaSpeed1;
speed2 = 5 * deltaSpeed2;
spuntoFlag = true;
PitMotor ();
//delay (10000);
Serial.print(" sto andando avanti ");
Serial.println();
}
}
if (irrecv.decode(&results)) // il ricevitore ha ricevuto un segnale IR ?
{
Serial.println(results.value, HEX); //UN Comment to see raw values
translateIR();
irrecv.resume(); // riceve il prossimo valore
}
// PitMotor (direction1, speed1, direction2, speed2);
delay (10);
}
// void PitMotor (int direction1, int speed1, int direction2, int speed2)
void PitMotor () // usa le variabili globali
{
// controllo delle velocità massime
if (speed1 > 255) { speed1 = 255 ;}
if (speed1 < 0) { speed1 = 0 ; }
if (speed2 > 255) { speed2 = 255; }
if (speed2 < 0) { speed2 = 0 ; }
// se l'interruttore 1 è chiuso il motore gira verso sinistra
if (direction1 == HIGH)
{
Serial.println("Motor1 : avanti");
Serial.print("speed1 : ");
Serial.println(speed1);
digitalWrite(motor1Pin, HIGH);
if (spuntoFlag == true || speed1 <=60 ){
Serial.println(" : spunto");
analogWrite(motor2Pin, 0); // spunto
delay(spuntoDelay);
}
analogWrite(motor2Pin, 255-speed1); // mantiene i valori con la rotazione invertita
}
// se l'interruttore 1 è aperto il motore gira verso destra
else {
Serial.println("Motor1 : indietro");
Serial.print("speed1 : ");
Serial.println(speed1);
digitalWrite(motor1Pin, LOW);
if (spuntoFlag == true || speed1 <=60 ){
Serial.println(" : spunto");
analogWrite(motor2Pin, 255); // spunto
delay(spuntoDelay);
}
analogWrite(motor2Pin, speed1);
}
// se l'interruttore 2 è chiuso il motore gira verso sinistra
if (direction2 == LOW)
{
Serial.println("Motor2 : avanti");
Serial.print("speed2 : ");
Serial.println(speed2);
digitalWrite(motor3Pin, HIGH);
if (spuntoFlag == true || speed2 <=60 ){
Serial.println(" : spunto");
analogWrite(motor4Pin, 0); // spunto
delay(spuntoDelay);
}
analogWrite(motor4Pin, 255-speed2); // mantiene i valori con la rotazione invertita
}
// se l'interruttore 2 è aperto il motore gira verso destra
else {
Serial.println("Motor2 : indietro");
Serial.print("speed2 : ");
Serial.println(speed2);
digitalWrite(motor3Pin, LOW);
if (spuntoFlag == true || speed2 <=60 ){
Serial.println(" : spunto");
analogWrite(motor4Pin, 255); // spunto
delay(spuntoDelay);
}
analogWrite(motor4Pin, speed2);
}
Serial.println(" ");
Serial.println(" --------------------------- ");
Serial.println(" ");
}
void PitTestaDistanza ()
{
// controllo ultrasuoni
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(5);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
// convert the time into a distance
cm = duration / 29 / 2;
if (flagDebug) {
Serial.print(trigPin);
Serial.print(" trigPin, ");
Serial.print(echoPin);
Serial.print(" echoPin, ");
Serial.print(duration);
Serial.print(" duration, ");
Serial.print(cm);
Serial.print("cm");
Serial.println();
}
if (cm >= maxDist) {
maxDist = cm;
maxAngle = servoPos;
}
if (cm <= minDist) {
minDist = cm;
minAngle = servoPos;
}
delay(1);
}
void PitTestaRotazione ()
{
if (servoPos >= servoMax)
{
servoPos = servoMax;
}
if (servoPos <= servoMin)
{
servoPos = servoMin;
}
myservo.write(servoPos); // tell servo to go to position in variable 'pos'
if (flagDebug) {
Serial.print("servoPos ");
Serial.print(servoPos);
Serial.println();
}
PitTestaDistanza ();
}
void PitRoverDirection ()
{
minDist = 9999;
maxDist = 0;
minAngle = 0;
maxAngle = 0;
for(servoPos = 90; servoPos >= servoMin; servoPos -= servoDelta)
{
PitTestaRotazione ();
delay(10);
}
for(servoPos = servoMin; servoPos <= servoMax; servoPos += servoDelta)
{
PitTestaRotazione ();
delay(10);
}
for(servoPos = servoMax; servoPos >= 90; servoPos -= servoDelta)
{
PitTestaRotazione ();
delay(10);
}
servoPos = 90;
PitTestaRotazione ();
if (flagDebug) {
Serial.print(minDist);
Serial.print(" minDist ");
Serial.print(minAngle);
Serial.print(" minAngle ");
Serial.print(maxDist);
Serial.print(" maxDist ");
Serial.print(maxAngle);
Serial.print(" maxAngle ");
Serial.println();
}
if (maxAngle >= 90)
{
Serial.print(" girare a destra ");
Serial.println();
}
else{
Serial.print(" girare a sinistra ");
Serial.println();
}
}
/*-----( Declare User-written Functions )-----*/
void translateIR() // takes action based on IR code received
// describing KEYES Remote IR codes
// DIGIQUEST 7310T
{
switch(results.value)
{
// - riga 1
case 0xFD9A65: Serial.println(" StanBy");
case 0x15629B25: Serial.println(" StanBy");
Serial.println(" Motor 2 - OFF");
enableMotor2 = LOW;
digitalWrite(enable2Pin, enableMotor2); // spegne motore 2
break;
case 0xFD1AE5: Serial.println(" Info");
case 0xBBB54321: Serial.println(" Info");
Serial.println(" Motor 2 - ON");
enableMotor2 = HIGH;
digitalWrite(enable2Pin, enableMotor2); // accende motore 2
spuntoFlag = true;
PitMotor ();
break;
case 0xFD18E7: Serial.println(" SubTitle");
case 0xC76EF4E5: Serial.println(" SubTitle");
Serial.println(" Motor 1 - ON");
enableMotor1 = HIGH;
digitalWrite(enable1Pin, enableMotor1); // accende motore 1
spuntoFlag = true;
PitMotor ();
break;
case 0xFD9867: Serial.println(" Mute");
case 0x57E346E1: Serial.println(" Mute");
Serial.println(" Motor 1 - OFF");
enableMotor1 = LOW;
digitalWrite(enable1Pin, enableMotor1); // spegne motore 1
break;
// - canali e volume
case 0xFDB24D: Serial.println(" CH +");
case 0x13F0CDE5: Serial.println(" CH +");
speed1 = speed1 + deltaSpeed1;
speed2 = speed2 + deltaSpeed2;
PitMotor ();
break;
case 0xFD8A75: Serial.println(" CH -");
case 0x2CD8C901: Serial.println(" CH -");
speed1 = speed1 - deltaSpeed1;
speed2 = speed2 - deltaSpeed2;
PitMotor ();
break;
case 0xFD708F: Serial.println(" RECALL");
case 0x6BFD8B01: Serial.println(" RECALL");
Serial.println(" Motor 1 - ON");
enableMotor1 = HIGH;
enableMotor2 = HIGH;
digitalWrite(enable1Pin, enableMotor1); // accende motore 1
Serial.println(" Motor 2 - ON");
digitalWrite(enable2Pin, enableMotor2); // accende motore 2
spuntoFlag = true;
PitMotor ();
break;
case 0xFD48B7: Serial.println(" PAUSE");
case 0x986FB325: Serial.println(" PAUSE");
Serial.println(" Motor 1 - OFF");
enableMotor1 = LOW;
enableMotor2 = LOW;
digitalWrite(enable1Pin, enableMotor1); // spegne motore 1
Serial.println(" Motor 2 - OFF");
digitalWrite(enable2Pin, enableMotor2); // spegne motore 2
break;
case 0xFDB04F: Serial.println(" VOL +");
case 0x42640C99: Serial.println(" VOL +");
speed1 = speed1 + deltaSpeed1;
speed2 = speed2 + deltaSpeed2;
PitMotor ();
break;
case 0xFD8877: Serial.println(" VOL -");
case 0x6F5974BD: Serial.println(" VOL -");
speed1 = speed1 - deltaSpeed1;
speed2 = speed2 - deltaSpeed2;
PitMotor ();
break;
case 0xFDA25D: Serial.println(" MENU");
case 0xCEBE4CC1: Serial.println(" MENU");
speed1 = speed1 + deltaSpeed1 / 2;
speed2 = speed2 - deltaSpeed2 / 2;
direction1 = HIGH;
direction2 = HIGH;
spuntoFlag = true;
PitMotor ();
break;
case 0xFDA05F: Serial.println(" EXIT");
case 0xA6B913BD: Serial.println(" EXIT");
speed1 = speed1 - deltaSpeed1 / 2;
speed2 = speed2 + deltaSpeed2 / 2;
direction1 = HIGH;
direction2 = HIGH;
spuntoFlag = true;
PitMotor ();
break;
// - spostamenti
case 0xFD609F: Serial.println(" FORWARD");
case 0xD0529225: Serial.println(" FORWARD");
speed1 = (speed1 + speed2) / 2;
if (speed1 == 0) {
speed1 = deltaSpeed1;
speed2 = deltaSpeed2;
}
direction1 = HIGH;
direction2 = HIGH;
spuntoFlag = true;
PitMotor ();
break;
case 0xFD5AA5: Serial.println(" LEFT");
case 0xC806D53D: Serial.println(" LEFT");
speed1 = speed1 + deltaSpeed1;
speed2 = speed2;
PitMotor ();
break;
case 0xFD58A7: Serial.println(" -OK-");
case 0x90C35B01: Serial.println(" -OK-");
speed1 = 0;
speed2 = 0;
PitMotor ();
break;
case 0xFDD827: Serial.println(" RIGHT");
case 0xD035B645: Serial.println(" RIGHT");
speed1 = speed1;
speed2 = speed2 + deltaSpeed2;
PitMotor ();
break;
case 0xFD6897: Serial.println(" REVERSE");
case 0x85E09D61: Serial.println(" REVERSE");
speed1 = (speed1 + speed2) / 2;
if (speed1 == 0) {
speed1 = deltaSpeed1;
speed2 = deltaSpeed2;
}
direction1 = LOW;
direction2 = LOW;
spuntoFlag = true;
PitMotor ();
break;
case 0xFDAA55: Serial.println(" EPG");
case 0x73A3CEBD: Serial.println(" EPG");
speed1 = speed1 + deltaSpeed1 / 2;
speed2 = speed2 - deltaSpeed2 / 2;
direction1 = LOW;
direction2 = LOW;
spuntoFlag = true;
PitMotor ();
break;
case 0xFDA857: Serial.println(" FAV");
case 0x25802501: Serial.println(" FAV");
speed1 = speed1 - deltaSpeed1 / 2;
speed2 = speed2 + deltaSpeed2 / 2;
direction1 = LOW;
direction2 = LOW;
spuntoFlag = true;
PitMotor ();
break;
case 0xFDC837: Serial.println(" PAGE +");
case 0xE7ABE421: Serial.println(" PAGE +");
break;
case 0xFDE817: Serial.println(" PAGE -");
case 0xD51CCE5D: Serial.println(" PAGE -");
break;
// OK -- Numeri
case 0xFD4AB5: Serial.println(" 1");
case 0xCFB32D61: Serial.println(" 1");
speed1 = 2 * deltaSpeed1;
speed2 = 2 * deltaSpeed2;
direction1 = HIGH;
direction2 = LOW;
spuntoFlag = true;
PitMotor ();
break;
case 0xFD0AF5: Serial.println(" 2");
case 0xD32B70FD: Serial.println(" 2");
speed1 = 2 * deltaSpeed1;
speed2 = 2 * deltaSpeed2;
direction1 = HIGH;
direction2 = HIGH;
spuntoFlag = true;
PitMotor ();
break;
case 0xFD08F7: Serial.println(" 3");
case 0xDEE522C1: Serial.println(" 3");
speed1 = 2 * deltaSpeed1;
speed2 = 2 * deltaSpeed2;
direction1 = LOW;
direction2 = HIGH;
spuntoFlag = true;
PitMotor ();
break;
case 0xFD6A95: Serial.println(" 4");
case 0x9D3D4D25: Serial.println(" 4");
autoPilotFlag = !autoPilotFlag;
Serial.print("AutoPilot ");
Serial.print(autoPilotFlag);
Serial.println();
break;
case 0xFD2AD5: Serial.println(" 5");
case 0xE32F7CC1: Serial.println(" 5");
autoPilotFlag = !autoPilotFlag;
Serial.print("AutoPilot ");
Serial.print(autoPilotFlag);
Serial.println();
break;
case 0xFD28D7: Serial.println(" 6");
case 0xCBD2CCFD: Serial.println(" 6");
autoPilotFlag = !autoPilotFlag;
Serial.print("AutoPilot ");
Serial.print(autoPilotFlag);
Serial.println();
break;
case 0xFD728D: Serial.println(" 7");
case 0x74514645: Serial.println(" 7");
deltaSpeed2 = deltaSpeed2 + 5;
speed2 = speed2 + 5;
PitMotor ();
break;
case 0xFD32CD: Serial.println(" 8");
case 0xBA4375E1: Serial.println(" 8");
deltaSpeed2 = deltaSpeed2 - 5;
speed2 = speed2 - 5;
PitMotor ();
break;
case 0xFD30CF: Serial.println(" 9");
case 0xB1EFBA9D: Serial.println(" 9");
deltaSpeed1 = deltaSpeed1 - 5;
speed1 = speed1 - 5;
PitMotor ();
break;
case 0xFDF00F: Serial.println(" 0");
case 0x6539FA7D: Serial.println(" 0");
deltaSpeed1 = deltaSpeed1 + 5;
speed1 = speed1 + 5;
PitMotor ();
break;
// OK -- ultime righe
case 0xFD52AD: Serial.println(" AUDIO");
case 0x168E56C1: Serial.println(" AUDIO");
servoPos = servoMin;
PitTestaRotazione ();
break;
case 0xFD12ED: Serial.println(" LANG");
case 0xA3CC4A5: Serial.println(" LANG");
servoPos = 90;
PitTestaRotazione ();
break;
case 0xFD10EF: Serial.println(" TV/RADIO");
case 0x1E90961: Serial.println(" TV/RADIO");
servoPos = 90;
PitTestaRotazione ();
break;
case 0xFDD02F: Serial.println(" TTX");
case 0xAAFCAC1: Serial.println(" TTX");
servoPos = servoMax ;
PitTestaRotazione ();
break;
case 0xFD629D: Serial.println(" ROSSO");
case 0x2F1EC521: Serial.println(" ROSSO");
servoPos = servoPos - servoDelta;
PitTestaRotazione ();
break;
case 0xFD22DD: Serial.println(" VERDE");
case 0x7510F4BD: Serial.println(" VERDE");
PitRoverDirection();
break;
case 0xFD20DF: Serial.println(" GIALLO");
case 0x1644C1C1: Serial.println(" GIALLO");
PitRoverDirection();
break;
case 0xFDE01F: Serial.println(" BLU");
case 0xC98F01A1: Serial.println(" BLU");
servoPos = servoPos + servoDelta;
PitTestaRotazione ();
break;
default:
Serial.println(" other button ");
}// End Case
spuntoFlag = false;
//Serial.print("spuntoFlag :");
//Serial.println(spuntoFlag);
delay(500); // Do not get immediate repeat
} //END translateIR
/* ( THE END ) */
ZappocoS, dicembre 2014 - gennaio 2015
