git.piffa.net Git - rover/blob - libraries/rover/rover.cpp

   1 /*
   2   Rover
   3
   4 Autore: Andrea Manni
   5
   6 Link: http://aero.piffa.net
   7 Licenza:    GPLv3
   8
   9 */
  10
  11 #include "Arduino.h"
  12 #include "rover.h"
  13
  14 #define dEBUG
  15
  16 // Configurazione parametri: variabili GLOBALI
  17 // motor one
  18 const int enA = 6;
  19 const int in1 = 7;
  20 const int in2 = 8;
  21 byte speedA = 255;
  22 // motor two
  23 const int enB = 5;
  24 const int in3 = 4;
  25 const int in4 = 3;
  26 byte speedB = 255;
  27
  28 // Servo vars
  29 int pos = 0;    // variable to store the servo position
  30 const byte servoPIN =9 ;
  31 const byte middle = 90; // Centratura servo
  32 const int spausa = 10; // Pausa movimenti servo
  33 const byte sx = 10;  // Min SX
  34 const byte dx = 170; // Maz DX
  35 Servo myservo; // Non c'e' bisogno di extern se e' dichiarato in questo scope
  36
  37 // Ultrasuoni
  38 const byte trigPIN = 11;
  39 const byte echoPIN= 12;
  40 const byte ledPIN = 13;
  41 long duration;
  42 int distance;
  43 const int minDistance = 10;
  44
  45 ////////////////////////
  46 // Funzioni:
  47 //
  48 void abilita() {
  49 // Abilita i PINs come OUTPUTS e attacca un servo
  50 // Motors
  51     pinMode(enA, OUTPUT);
  52     pinMode(in1, OUTPUT);
  53     pinMode(in2, OUTPUT);
  54     pinMode(enB, OUTPUT);
  55     pinMode(in3, OUTPUT);
  56     pinMode(in4, OUTPUT);
  57 // Servo
  58     pinMode(servoPIN, OUTPUT);
  59     myservo.attach(servoPIN); //la libreria servo deve essere sempre inclusa
  60 // Ultrasonic
  61         pinMode(trigPIN, OUTPUT);
  62     pinMode(echoPIN, INPUT);
  63     pinMode(ledPIN, OUTPUT);
  64 }
  65
  66
  67 // MotorA
  68 void forwardA() {
  69     // Avanzamento motore
  70     digitalWrite(in1,LOW);
  71     digitalWrite(in2,HIGH);
  72     analogWrite(enA,speedA);
  73 }
  74
  75 void forwardA(byte speedA) {
  76     // Avanzamento motore
  77     digitalWrite(in1,LOW);
  78     digitalWrite(in2,HIGH);
  79     analogWrite(enA,speedA);
  80 }
  81
  82 void backwardA() {
  83     // Reverse motore
  84     digitalWrite(in2,LOW);
  85     digitalWrite(in1,HIGH);
  86     analogWrite(enA,speedA);
  87 }
  88
  89 void backwardA(byte speedA) {
  90     // Reverse motore
  91     digitalWrite(in2,LOW);
  92     digitalWrite(in1,HIGH);
  93     analogWrite(enA,speedA);
  94 }
  95
  96 void stopA() {
  97     // Stop
  98     digitalWrite(enA,LOW);
  99 }
 100
 101 // MotorB
 102 void forwardB() {
 103     // Avanzamento motore
 104     digitalWrite(in3,LOW);
 105     digitalWrite(in4,HIGH);
 106     analogWrite(enB,speedB);
 107 }
 108
 109 void forwardB(byte speedB) {
 110     // Avanzamento motore
 111     digitalWrite(in3,LOW);
 112     digitalWrite(in4,HIGH);
 113     analogWrite(enB,speedB);
 114 }
 115
 116 void backwardB() {
 117     // Reverse motore
 118     digitalWrite(in4,LOW);
 119     digitalWrite(in3,HIGH);
 120     analogWrite(enB,speedB);
 121 }
 122
 123 void backwardB(byte speedB) {
 124     // Reverse motore
 125     digitalWrite(in4,LOW);
 126     digitalWrite(in3,HIGH);
 127     analogWrite(enB,speedB);
 128 }
 129
 130 void stopB() {
 131     // Stop
 132     digitalWrite(enB,LOW);
 133 }
 134
 135
 136 // Entrambi i motori
 137 void avanti() {
 138     // Drive ahead: funzione composita
 139     forwardA() ;
 140     forwardB() ;
 141 }
 142 void indietro() {
 143     // Drive backward: funzione composita
 144     backwardA();
 145     backwardB();
 146 }
 147 void giraSX() {
 148     // Gira a DX
 149     forwardB() ;
 150     backwardA();
 151 }
 152 void giraDX()  {
 153     // Gira a DX
 154     forwardA() ;
 155     backwardB();
 156 }
 157 void stop()  {
 158     stopA();
 159     stopB();
 160 }
 161
 162
 163
 164 // Servo
 165 void servoDX() {
 166     // TurnDX
 167     while (pos < dx) {
 168         myservo.write(pos++);
 169         delay(spausa);
 170     }
 171 }
 172
 173 void servoSX() {
 174     // TurnSX
 175     while (pos > sx) {
 176         myservo.write(pos--);
 177         delay(spausa);
 178     }
 179 }
 180
 181 void servoMiddle() {
 182     // Middle
 183     while (pos > middle) {
 184         myservo.write(pos--);
 185         delay(spausa);
 186     }
 187     while (pos < middle) {
 188         myservo.write(pos++);
 189         delay(spausa);
 190     }
 191 }
 192
 193
 194 // Ultrasuoni
 195 boolean distanceCheck() {
 196 // Verifica se la distanza di un oggetto e' minore di minDistance
 197     digitalWrite(trigPIN, LOW);  // Prepare for ping
 198     delayMicroseconds(2); //
 199     digitalWrite(trigPIN, HIGH); // Send a ping
 200     delayMicroseconds(10); //
 201     digitalWrite(trigPIN, LOW); // Set down ping
 202     duration = pulseIn(echoPIN, HIGH);
 203     //distance = (duration / 2) / 29.1; // Speed is ~300m/s,
 204     // so it takes ~29.1 milliseconds for a cm.
 205     distance = (duration / 58.2); // Atmegas are not found of divisions
 206     // Distance is half of (out + back)
 207 #ifdef DEBUG
 208 Serial.print("Distanza oggetto: ");
 209 Serial.println(distance);
 210 #endif
 211     if (distance < minDistance) {  // This is where the LED On/Off happens
 212         return 1;
 213     }
 214     else {
 215         return 0;
 216     }
 217 }
 218
 219 int distanceMonitor() {
 220 // Ritorna la distanza di un oggetto in cm
 221     digitalWrite(trigPIN, LOW);  // Prepare for ping
 222     delayMicroseconds(2); //
 223     digitalWrite(trigPIN, HIGH); // Send a ping
 224     delayMicroseconds(10); //
 225     digitalWrite(trigPIN, LOW); // Set down ping
 226     duration = pulseIn(echoPIN, HIGH);
 227     //distance = (duration / 2) / 29.1; // Speed is ~300m/s,
 228     // so it takes ~29.1 milliseconds for a cm.
 229     distance = (duration / 58.2); // Atmegas are not found of divisions
 230     // Distance is half of (out + back)
 231
 232     return distance;
 233 }