kevinveenbirkenbach
/
formiko
mirror of https://github.com/kevinveenbirkenbach/formiko.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Motorsteurung optimiert

This commit is contained in:
Kevin Frantz 2017-01-18 22:14:27 +01:00
parent 0dff93794c
commit 1c7ab97b9d
1 changed files with 65 additions and 85 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

150
formiko.ino
View File

@ -11,15 +11,16 @@
* @since 2017-01-16 * @since 2017-01-16
**/ **/
//include "Communication.h"
//Bibliothek für Ultraschalsensor //Bibliothek für Ultraschalsensor
#include <NewPing.h> #include <NewPing.h>
//Bibliothek für Motorsteurung
#include <Motor.h>
// Ultraschallsensor // Ultraschallsensor
#define TRIGGER_PIN 3 #define TRIGGER_PIN 3
#define ECHO_PIN 4 #define ECHO_PIN 4
#define MAX_DISTANCE 400 #define MAX_DISTANCE 200
// Infarosensoren // Infarosensoren
#define INFAROT_LEFT 2 #define INFAROT_LEFT 2
@ -35,55 +36,44 @@
#define MOTOR_RIGHT_PWM 7 #define MOTOR_RIGHT_PWM 7
#define MOTOR_PWM_MIN 10 // arbitrary slow speed PWM duty cycle #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(){ 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 //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 //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 //Dreht den Roboter nach rechts
void turnRight(){ void turnRight(){
turnWheel(MOTOR_LEFT_DIRECTION,MOTOR_LEFT_PWM); motorLeft.forward();
motorRight.backward();
} }
//Dreht den Roboter nach links //Dreht den Roboter nach links
void turnLeft(){ void turnLeft(){
turnWheel(MOTOR_RIGHT_DIRECTION,MOTOR_RIGHT_PWM); motorLeft.backward();
motorRight.forward();
} }
//Gibt die Sensordaten über die serielle Schnittstelle aus //Gibt die Sensordaten über die serielle Schnittstelle aus
@ -96,40 +86,38 @@ void serialStatus(){
Serial.println(digitalRead(INFAROT_RIGHT)); 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 //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++){ for(int i=0;i<count;i++){
digitalWrite(led, HIGH); digitalWrite(led, HIGH);
delay(time); delay(time);
digitalWrite(led, LOW); // turn the LED off by making the voltage LOW 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 //Stellt ein Serielles Testinterface zur Verfügung
void siMenue(){ void siMenue(){
byte c; byte c;
@ -143,17 +131,17 @@ void siMenue(){
Serial.println( "3) Turn Right "); Serial.println( "3) Turn Right ");
Serial.println( "4) Stop "); Serial.println( "4) Stop ");
Serial.println( "5) Show Status "); Serial.println( "5) Show Status ");
Serial.println( "6) Drive Backwards ");
Serial.println( "" ); Serial.println( "" );
Serial.println( "@author kf" ); Serial.println( "@author kf" );
Serial.println( "@since 2016-01-16" ); Serial.println( "@since 2016-01-16" );
Serial.println( "-----------------------------" ); Serial.println( "-----------------------------" );
Serial.print( "?" );
//while(true){
do{ do{
isValidInput = true; isValidInput = true;
// get the next character from the serial port
Serial.print( "?" );
//Abwarten bis serielles Interface zur Verfügung steht //Abwarten bis serielles Interface zur Verfügung steht
//while( !Serial.available() ); while( !Serial.available() );
c = Serial.read(); c = Serial.read();
switch( c ) switch( c )
{ {
@ -172,13 +160,19 @@ void siMenue(){
case '5': case '5':
serialStatus(); serialStatus();
break; break;
case '6':
driveBackward();
break;
default: default:
Serial.println("Die gewünschte Funktion steht nicht zur Verfügung!"); //isValidInput = false;
break; break;
} }
delay(1500); //delay(1500);
} while(true); } while(isValidInput);
//Serial.println("Die gewünschte Funktion steht nicht zur Verfügung!");
//}
} }
//Initialisierung //Initialisierung
void setup(){ void setup(){
//LED initialisierung //LED initialisierung
@ -189,34 +183,20 @@ void setup(){
pinMode(INFAROT_LEFT,INPUT); pinMode(INFAROT_LEFT,INPUT);
pinMode(INFAROT_RIGHT,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. //Open serial monitor at 115200 baud to see ping results.
Serial.begin(115200); Serial.begin(115200);
//Randomwerte zur verfügung stellen //Randomwerte zur verfügung stellen
randomSeed(analogRead(0)); randomSeed(analogRead(0));
} }
//Main-Loop //Main-Loop
void loop(){ void loop(){
//blinkLED(EXTERNAL_LED);
//digitalWrite(INTERNAL_LED, HIGH);
//Solange Standartroutinen ausführen bis USB geladen //Solange Standartroutinen ausführen bis USB geladen
//while( !Serial.available() ){ while( !Serial.available() ){
// randomDrive(); randomDrive();
//} }
//Serieles Menue aufrufen //Serieles Menue aufrufen
siMenue(); siMenue();
//serialStatus();
} }