Servo func clean up, drive lib

[rover] / libraries / rover / rover.cpp

diff --git a/libraries/rover/rover.cpp b/libraries/rover/rover.cpp
index 5328a02bf8328a8388edabac9e4c724faa7d2e70..58bbf2cbb082a1f3e2b2c685f8ff1e675df8e311 100644 (file)
--- a/libraries/rover/rover.cpp
+++ b/libraries/rover/rover.cpp
@@ -1,5 +1,5 @@
 /*
-  Rover 
+  Rover
 
 Autore: Andrea Manni
 
@@ -24,8 +24,19 @@ const int enB = 5;
 const int in3 = 4;
 const int in4 = 3;
 byte speedB = 255;
-// Funzioni
 
+// Servo vars
+int pos = 0;    // variable to store the servo position
+const byte servoPIN =9 ;
+const byte middle = 90; // Centratura servo
+const int spausa = 10; // Pausa movimenti servo
+const byte sx = 10;  // Min SX
+const byte dx = 170; // Maz DX
+Servo myservo; // Non c'e' bisogno di extern se e' dichiarato in questo scope
+
+////////////////////////
+// Funzioni:
+//
 void abilita() {
 // Abilita i PINs come OUTPUTS
     pinMode(enA, OUTPUT);
@@ -34,10 +45,14 @@ void abilita() {
     pinMode(enB, OUTPUT);
     pinMode(in3, OUTPUT);
     pinMode(in4, OUTPUT);
+
+
+    pinMode(servoPIN, OUTPUT);
+    myservo.attach(servoPIN);
 }
 
 
-// MotorB
+// MotorA
 void forwardA() {
     // Avanzamento motore
     digitalWrite(in1,LOW);
@@ -45,6 +60,13 @@ void forwardA() {
     analogWrite(enA,speedA);
 }
 
+void forwardA(byte speedA) {
+    // Avanzamento motore
+    digitalWrite(in1,LOW);
+    digitalWrite(in2,HIGH);
+    analogWrite(enA,speedA);
+}
+
 void backwardA() {
     // Reverse motore
     digitalWrite(in2,LOW);
@@ -52,6 +74,13 @@ void backwardA() {
     analogWrite(enA,speedA);
 }
 
+void backwardA(byte speedA) {
+    // Reverse motore
+    digitalWrite(in2,LOW);
+    digitalWrite(in1,HIGH);
+    analogWrite(enA,speedA);
+}
+
 void stopA() {
     // Stop
     digitalWrite(enA,LOW);
@@ -65,6 +94,13 @@ void forwardB() {
     analogWrite(enB,speedB);
 }
 
+void forwardB(byte speedB) {
+    // Avanzamento motore
+    digitalWrite(in3,LOW);
+    digitalWrite(in4,HIGH);
+    analogWrite(enB,speedB);
+}
+
 void backwardB() {
     // Reverse motore
     digitalWrite(in4,LOW);
@@ -72,7 +108,72 @@ void backwardB() {
     analogWrite(enB,speedB);
 }
 
+void backwardB(byte speedB) {
+    // Reverse motore
+    digitalWrite(in4,LOW);
+    digitalWrite(in3,HIGH);
+    analogWrite(enB,speedB);
+}
+
 void stopB() {
     // Stop
     digitalWrite(enB,LOW);
 }
+
+
+// Entrambi i motori
+void avanti() {
+    // Drive ahead: funzione composita
+    forwardA() ;
+    forwardB() ;
+}
+void indietro() {
+    // Drive backward: funzione composita
+    backwardA();
+    backwardB();
+}
+void giraSX() {
+    // Gira a DX
+    forwardB() ;
+    backwardA();
+}
+void giraDX()  {
+    // Gira a DX
+    forwardA() ;
+    backwardB();
+}
+void stop()  {
+    stopA();
+    stopB();
+}
+
+
+
+// Servo
+void servoDX() {
+    // TurnDX
+    while (pos < dx) {
+        myservo.write(pos++);
+        delay(spausa);
+    }
+}
+
+void servoSX() {
+    // TurnSX
+    while (pos > sx) {
+        myservo.write(pos--);
+        delay(spausa);
+    }
+}
+
+void servoMiddle() {
+    // Middle
+    while (pos > middle) {
+        myservo.write(pos--);
+        delay(spausa);
+    }
+    while (pos < middle) {
+        myservo.write(pos++);
+        delay(spausa);
+    }
+}