Motorsteurung optimiert

formiko.ino

@@ -11,15 +11,16 @@
  * @since 2017-01-16
  **/
 
-
-//include "Communication.h"
 //Bibliothek für Ultraschalsensor
 #include <NewPing.h>
 
+//Bibliothek für Motorsteurung
+#include <Motor.h>
+
 // Ultraschallsensor
 #define TRIGGER_PIN   3  
 #define ECHO_PIN      4  
-#define MAX_DISTANCE  400 
+#define MAX_DISTANCE  200 
 
 // Infarosensoren
 #define INFAROT_LEFT  2
@@ -35,55 +36,44 @@
 #define MOTOR_RIGHT_PWM 7
 
 #define MOTOR_PWM_MIN 10            // arbitrary slow speed PWM duty cycle
-#define MOTOR_PWM_MAX 200           // arbitrary fast speed PWM duty cycle
+#define MOTOR_PWM_NORMAL 50
-#define MOTOR_DIRECTION_DELAY 200  // brief delay for abrupt motor changes
+#define MOTOR_DIRECTION_DELAY 50  // brief delay for abrupt motor changes
 
-NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE); // NewPing setup of pins and maximum distance.
+//Initialisierung des Ultraschalsonars
+NewPing sonar(TRIGGER_PIN, ECHO_PIN, MAX_DISTANCE);
 
-//Dreht ein Rad des Roboters
+//Initialisierung der Motoren
-void turnWheel(int directionPin, int pwmPin, int directionValue=LOW, int pwmValue=MOTOR_PWM_MIN){ 
+Motor motorLeft = Motor(MOTOR_LEFT_DIRECTION,MOTOR_LEFT_PWM,HIGH,MOTOR_PWM_NORMAL,MOTOR_DIRECTION_DELAY);
-  digitalWrite(directionPin,directionValue); // direction = forward
+Motor motorRight = Motor(MOTOR_RIGHT_DIRECTION,MOTOR_RIGHT_PWM,HIGH,MOTOR_PWM_NORMAL,MOTOR_DIRECTION_DELAY);
-  analogWrite(pwmPin, 255-pwmValue);         // PWM speed = fast
-}
 
-// Stoppt ein Roboterrad
+//Stopt die Motoren
-void stopWheel(int directionPin, int pwmPin){
-  digitalWrite(directionPin, LOW );
-  digitalWrite(pwmPin, LOW );
-}
-
-//Stopt das Roboterrad
 void stopMotors(){
-  stopWheel(MOTOR_LEFT_DIRECTION,MOTOR_LEFT_PWM);
+  motorLeft.stop();
-  stopWheel(MOTOR_RIGHT_DIRECTION,MOTOR_RIGHT_PWM);
+  motorRight.stop();
-  
-  //Abwarten bis Roboter gestoppt wurde
-  delay(MOTOR_DIRECTION_DELAY);
 }
 
 //Fährt den Roboter nach vorne
-void driveForward(int motorSpeed=MOTOR_PWM_MIN){
+void driveForward(){
-  turnWheel(MOTOR_LEFT_DIRECTION,MOTOR_LEFT_PWM,LOW,motorSpeed);
+  motorLeft.forward();
-  turnWheel(MOTOR_RIGHT_DIRECTION,MOTOR_RIGHT_PWM,LOW,motorSpeed);
+  motorRight.forward();
-  
 }
 
 //Fährt den Roboter zurück
-void driveBackwards(int motorSpeed=MOTOR_PWM_MIN){
+void driveBackward(){
-  motorSpeed+=255;
+  motorLeft.backward();
-  turnWheel(MOTOR_RIGHT_DIRECTION,MOTOR_RIGHT_PWM,HIGH,motorSpeed);
+  motorRight.backward();
-  turnWheel(MOTOR_LEFT_DIRECTION,MOTOR_LEFT_PWM,HIGH,motorSpeed);
-  
 }
 
 //Dreht den Roboter nach rechts
 void turnRight(){
-  turnWheel(MOTOR_LEFT_DIRECTION,MOTOR_LEFT_PWM);
+  motorLeft.forward();
+  motorRight.backward();
 }
 
 //Dreht den Roboter nach links
 void turnLeft(){
-  turnWheel(MOTOR_RIGHT_DIRECTION,MOTOR_RIGHT_PWM);
+  motorLeft.backward();
+  motorRight.forward();
 }
 
 //Gibt die Sensordaten über die serielle Schnittstelle aus
@@ -96,40 +86,38 @@ void serialStatus(){
   Serial.println(digitalRead(INFAROT_RIGHT));
 }
 
-//Fährt den Roboter zuällig durch den Raum und umgeht Hindernisse
-void randomDrive(){
-  //Prüfen ob Objekt in Fahrbahn
-  if(sonar.ping_cm()<=40 && sonar.ping_cm()){
-    if(sonar.ping_cm()<=20){
-      //Rückwärtsgang einlegen wenn zu nah an Objekt
-      driveBackwards();
-    }else{
-      //Zufälligerweise in die eine, oder andere Richtung drehen
-      if(random(2)){
-        turnLeft();
-      }else{
-        turnRight();
-      }
-    }
-  }else{
-    driveForward(MOTOR_PWM_MIN);
-  }
-  //Zeit für Aktionen gewähren
-  delay(MOTOR_DIRECTION_DELAY);
-  
-  //Motoren nach Aktion stoppen
-  stopMotors();
-}
-
 //Lässt eine LED blinken
-void blinkLED(int led,int count=7, int time=1000){
+void blinkLED(int led,int count=1, int time=1000){
   for(int i=0;i<count;i++){  
     digitalWrite(led, HIGH);
     delay(time);               
     digitalWrite(led, LOW);    // turn the LED off by making the voltage LOW
+    delay(time);
   }
 }
 
+//Fährt den Roboter zuällig durch den Raum und umgeht Hindernisse
+void randomDrive(){
+  //Prüfen ob Objekt in Fahrbahn
+  if(sonar.ping_cm()<=20 && sonar.ping_cm()!=0){
+   if(sonar.ping_cm()<=10){  
+     driveBackward();
+   }else
+   //Zufälligerweise in die eine, oder andere Richtung drehen
+   if(random(2)){
+      turnLeft();
+   }else{
+     turnRight();
+   }
+  }else{
+    driveForward();
+  }
+  delay(1000);
+  //Motoren nach Aktion stoppen
+  stopMotors();
+  blinkLED(INTERNAL_LED);
+}
+
 //Stellt ein Serielles Testinterface zur Verfügung
 void siMenue(){
   byte c;
@@ -143,17 +131,17 @@ void siMenue(){
   Serial.println( "3) Turn Right ");
   Serial.println( "4) Stop ");
   Serial.println( "5) Show Status ");
+  Serial.println( "6) Drive Backwards ");
   Serial.println( "" );
   Serial.println( "@author kf" );
   Serial.println( "@since 2016-01-16" );
   Serial.println( "-----------------------------" );
-    
+  Serial.print( "?" );
+//while(true){    
   do{
     isValidInput = true;
-    // get the next character from the serial port
-    Serial.print( "?" );
     //Abwarten bis serielles Interface zur Verfügung steht
-    //while( !Serial.available() );
+    while( !Serial.available() );
     c = Serial.read();
     switch( c )
     {
@@ -172,13 +160,19 @@ void siMenue(){
       case '5':      
         serialStatus();
         break;
+      case '6':      
+        driveBackward();
+        break;
       default:
-        Serial.println("Die gewünschte Funktion steht nicht zur Verfügung!");
+        //isValidInput = false;
         break;
     }
-    delay(1500);
+    //delay(1500);
-  } while(true); 
+  } while(isValidInput);
+//Serial.println("Die gewünschte Funktion steht nicht zur Verfügung!");         
+//}
 }
+
 //Initialisierung
 void setup(){
   //LED initialisierung
@@ -189,34 +183,20 @@ void setup(){
   pinMode(INFAROT_LEFT,INPUT);
   pinMode(INFAROT_RIGHT,INPUT);
   
-  //Initialisierung des linken Motors
-  pinMode(MOTOR_LEFT_DIRECTION, OUTPUT );
-  pinMode(MOTOR_LEFT_PWM, OUTPUT );
-  digitalWrite(MOTOR_LEFT_DIRECTION, LOW );
-  digitalWrite(MOTOR_LEFT_PWM, LOW );
-
-  //Initialisierung des rechten Motors
-  pinMode(MOTOR_RIGHT_DIRECTION, OUTPUT );
-  pinMode(MOTOR_RIGHT_PWM, OUTPUT );
-  digitalWrite(MOTOR_RIGHT_DIRECTION, LOW );
-  digitalWrite(MOTOR_RIGHT_PWM, LOW );
-
   //Open serial monitor at 115200 baud to see ping results.
   Serial.begin(115200);
 
   //Randomwerte zur verfügung stellen
   randomSeed(analogRead(0));
+  
 }
 
 //Main-Loop
 void loop(){
-  //blinkLED(EXTERNAL_LED);
-  //digitalWrite(INTERNAL_LED, HIGH);
   //Solange Standartroutinen ausführen bis USB geladen 
-  //while( !Serial.available() ){
+  while( !Serial.available() ){
-  //   randomDrive();
+     randomDrive();
-  //}
+  }
   //Serieles Menue aufrufen
   siMenue();
-  //serialStatus();
 }