final
This commit is contained in:
reto
241
follower_bot_circle.ino.ino_save
Normal file
241
follower_bot_circle.ino.ino_save
Normal file
@ -0,0 +1,241 @@
|
||||
/******************************************************************************
|
||||
// FreakyBot Circle
|
||||
/******************************************************************************/
|
||||
|
||||
/******************************************************************************
|
||||
/* Konfigruation
|
||||
/******************************************************************************/
|
||||
//pins
|
||||
|
||||
#define MDL 4 // Motor Direction Left
|
||||
#define MSL 5 // Motor Speed Left
|
||||
#define MSR 6 // Motor Speed Right
|
||||
#define MDR 7 // Motor Direction Right
|
||||
|
||||
int IrLedVr = 3;
|
||||
int IrLedVl = 2;
|
||||
int IrLedHr = 10;
|
||||
int IrLedHl = 9;
|
||||
|
||||
int IrRecVr = A3;
|
||||
int IrRecVl = A2;
|
||||
int IrRecHr = A0;
|
||||
int IrRecHl = A1;
|
||||
const int ledPin = 13; // the number of the LED pin
|
||||
|
||||
#define DIST 65 // Define Minimum Distance
|
||||
|
||||
// dino
|
||||
/*
|
||||
#define MTRF 200 // Motor Topspeed Right Forward
|
||||
#define MTRB 200 // Motor Topspeed Right Backward
|
||||
#define MTLF 170 // Motor Topspeed Left Forward
|
||||
#define MTLB 180 // Motor Topspeed Left Backward
|
||||
*/
|
||||
|
||||
// normal
|
||||
#define MTRF 200 // Motor Topspeed Right Forward
|
||||
#define MTLB 170 // Motor Topspeed Left Backward / Forw
|
||||
|
||||
#define MTRB 200 // Motor Topspeed Right Backward
|
||||
#define MTLF 170 // Motor Topspeed Left Forward / Backw
|
||||
|
||||
// Variables
|
||||
int lspeed = 0; // current speed of left motor
|
||||
int rspeed = 0; // current speed of right motor
|
||||
int spause = 1; // wait time if a whisker was touched
|
||||
long delay_time = 0; // time when pause was triggered
|
||||
int ledState = LOW; // ledState used to set the LED
|
||||
long previousMillis = 0; // will store last time LED was updated
|
||||
long interval = 5000; // interval at which to blink (milliseconds)
|
||||
bool protsta = false;
|
||||
bool motsta = true;
|
||||
|
||||
/******************************************************************************
|
||||
/* Setup
|
||||
/******************************************************************************/
|
||||
void setup () {
|
||||
//motors
|
||||
pinMode (MDL, OUTPUT);
|
||||
pinMode (MSL, OUTPUT);
|
||||
pinMode (MSR, OUTPUT);
|
||||
pinMode (MDR, OUTPUT);
|
||||
// all IR Leds / Recevier
|
||||
pinMode(IrLedVr, OUTPUT);
|
||||
pinMode(IrLedVl, OUTPUT);
|
||||
pinMode(IrLedHr, OUTPUT);
|
||||
pinMode(IrLedHl, OUTPUT);
|
||||
pinMode(IrRecVr, INPUT);
|
||||
pinMode(IrRecVl, INPUT);
|
||||
pinMode(IrRecHr, INPUT);
|
||||
pinMode(IrRecHl, INPUT);
|
||||
// Serial Interface
|
||||
Serial.begin(57600);
|
||||
Serial.println(F("Robot Starting"));
|
||||
delay(500);
|
||||
}
|
||||
|
||||
/******************************************************************************
|
||||
/* Hauptprogramm
|
||||
/******************************************************************************/
|
||||
void loop () {
|
||||
|
||||
// left / right
|
||||
for ( int i = 0; i <=73; i++ ){
|
||||
drive(110,165);
|
||||
delay(100);
|
||||
}
|
||||
|
||||
for ( int i = 0; i <=73; i++ ){
|
||||
drive(165,110);
|
||||
delay(100);
|
||||
}
|
||||
}
|
||||
|
||||
/******************************************************************************
|
||||
/* Funktionen
|
||||
/******************************************************************************/
|
||||
|
||||
/******************************************************************************
|
||||
/* Funktion: Drive
|
||||
/******************************************************************************/
|
||||
void drive( int left, int right)
|
||||
{
|
||||
int lstate = LOW;
|
||||
int rstate = LOW;
|
||||
|
||||
left = constrain(left,-255,255);
|
||||
right = constrain(right,-255,255);
|
||||
|
||||
// invert left
|
||||
left = left * -1;
|
||||
// invert right
|
||||
//right = right * -1;
|
||||
|
||||
if (left >= 0)
|
||||
{
|
||||
// remap the motor value to the calibrated interval
|
||||
// speed is alwas handled in the range of [-255..255]
|
||||
left = map(left,0,255,0,MTLF);
|
||||
}
|
||||
else
|
||||
{
|
||||
lstate = HIGH;
|
||||
left = 255 - map(-left,0,255,0,MTLB);
|
||||
}
|
||||
if (right >= 0)
|
||||
{
|
||||
right = map(right,0,255,0,MTRF);
|
||||
}
|
||||
else
|
||||
{
|
||||
rstate = HIGH;
|
||||
right = 255 - map(-right,0,255,0,MTRB);
|
||||
}
|
||||
analogWrite(MSL, left);
|
||||
digitalWrite(MDL, lstate);
|
||||
analogWrite(MSR, right);
|
||||
digitalWrite(MDR, rstate);
|
||||
|
||||
}
|
||||
|
||||
/******************************************************************************
|
||||
* Funktion: irRead (a,b)
|
||||
* a: Pinnummer des Ir Empfängers
|
||||
* b: Pinnummer des Ir Senders
|
||||
* c: Modus: Space = Normalmodus returns HIGH if > DIST
|
||||
* d = Distancemodus return Distance 0 to 100 mm
|
||||
* Retourwert: HIGH = IR-Licht detektiert
|
||||
* d.h. gemessener Wert unterscheidet sich zum Umgebungslicht
|
||||
******************************************************************************/
|
||||
int irRead(int readPin, int triggerPin, char mode)
|
||||
{
|
||||
boolean zustand;
|
||||
int umgebungswert = 0;
|
||||
int irwert =0;
|
||||
float uLichtzuIr;
|
||||
|
||||
for (int i=0;i<10;i++) {
|
||||
digitalWrite(triggerPin, LOW);
|
||||
delay(5);
|
||||
umgebungswert = umgebungswert + analogRead(readPin);
|
||||
digitalWrite(triggerPin, HIGH);
|
||||
delay(5);
|
||||
irwert = irwert + analogRead(readPin);
|
||||
}
|
||||
|
||||
// Remove Umgebungswert
|
||||
irwert = irwert - umgebungswert;
|
||||
|
||||
// detect obstacle
|
||||
if(irwert>DIST){
|
||||
zustand = HIGH;
|
||||
}else{
|
||||
zustand = LOW;
|
||||
}
|
||||
|
||||
// for Debug
|
||||
if(protsta && zustand || mode == 'd'){
|
||||
/* Serial.print("tr ");
|
||||
Serial.print(triggerPin);
|
||||
Serial.print(" r ");
|
||||
Serial.print(readPin);
|
||||
Serial.print(" um ");
|
||||
Serial.print(umgebungswert);
|
||||
Serial.print(" ir ");
|
||||
Serial.print(irwert);
|
||||
Serial.print(" % ");
|
||||
Serial.print(uLichtzuIr); */
|
||||
|
||||
delay(5);
|
||||
if(mode == 'd'){
|
||||
// Serial.print(" irwert ");
|
||||
// Serial.print(irwert);
|
||||
irwert = map(irwert, 0, 300, 100, -100);
|
||||
// Serial.print(" dis ");
|
||||
// Serial.println(irwert);
|
||||
};
|
||||
return irwert;
|
||||
} else {
|
||||
return zustand;
|
||||
// Serial.print(" zu ");
|
||||
// Serial.println(zustand);
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
// CHECKSERIAL)
|
||||
//*****************************************************************************
|
||||
void checkserial(){
|
||||
if ( Serial.available() > 0 ) {
|
||||
// Read the incoming byte
|
||||
char theChar = Serial.read();
|
||||
// Parse character
|
||||
switch (theChar) {
|
||||
case 'p':
|
||||
Serial.println(F("protocoll on/off"));
|
||||
delay(5);
|
||||
if (protsta){
|
||||
protsta = false;
|
||||
}else{
|
||||
protsta = true;
|
||||
}
|
||||
delay(5);
|
||||
break;
|
||||
case 'm':
|
||||
delay(10);
|
||||
if (motsta){
|
||||
motsta = false;
|
||||
}else{
|
||||
motsta = true;
|
||||
}
|
||||
delay(5);
|
||||
break; default:
|
||||
// Display error message
|
||||
Serial.print(F("ERROR: Command not found, it was: "));
|
||||
Serial.println(theChar);
|
||||
Serial.println(F("p=protocoll on/off;m=motor on/off"));
|
||||
break;
|
||||
}
|
||||
}
|
||||
} // void checkserial
|
||||
Reference in New Issue
Block a user