336 lines
8.4 KiB
Plaintext
336 lines
8.4 KiB
Plaintext
|
/******************************************************************************
|
|||
|
|
// FreakyBot Follower bot formula
|
|||
|
|
/************************* *****************************************************/
|
|||
|
|
/* Speed = ( Distance – 0.3 ) / 0.7
|
|||
|
|
/******************************************************************************
|
|||
|
|
|
|||
|
|
/******************************************************************************
|
|||
|
|
/* Configuration
|
|||
|
|
/******************************************************************************/
|
|||
|
|
|
|||
|
|
/* Pin Configuration
|
|||
|
|
/******************************************************************************/
|
|||
|
|
#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
|
|||
|
|
|
|||
|
|
/* MOTOR DIFFERENCE CONFIG
|
|||
|
|
/******************************************************************************/
|
|||
|
|
// 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
|
|||
|
|
float lspeed = 0; // current speed of left motor
|
|||
|
|
float 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
|
|||
|
|
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(2000);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/******************************************************************************
|
|||
|
|
/* Hauptprogramm
|
|||
|
|
/******************************************************************************/
|
|||
|
|
void loop () {
|
|||
|
|
delay(5);
|
|||
|
|
checkserial();
|
|||
|
|
|
|||
|
|
/* Normal Way forward
|
|||
|
|
if ( irRead(IrRecVl,IrLedVl,' ')==HIGH &&
|
|||
|
|
irRead(IrRecVr,IrLedVr,' ')==HIGH )
|
|||
|
|
{
|
|||
|
|
// left and right whisker touched, turn both motor backwards left faster
|
|||
|
|
rspeed = -128;
|
|||
|
|
lspeed = -150;
|
|||
|
|
spause = 500;
|
|||
|
|
digitalWrite(ledPin,HIGH);
|
|||
|
|
delay(10);
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
else if (irRead(IrRecVl,IrLedVl,' ')==HIGH)
|
|||
|
|
{
|
|||
|
|
// left whisker touched, turn right motor backwards
|
|||
|
|
rspeed = -128;
|
|||
|
|
spause = 500;
|
|||
|
|
digitalWrite(ledPin,HIGH);
|
|||
|
|
delay(10);
|
|||
|
|
}
|
|||
|
|
else if (irRead(IrRecVr,IrLedVr,' ')==HIGH)
|
|||
|
|
{
|
|||
|
|
// right whisker touched, turn left motor backwards
|
|||
|
|
lspeed = -128;
|
|||
|
|
spause = 500;
|
|||
|
|
digitalWrite(ledPin,HIGH);
|
|||
|
|
delay(10);
|
|||
|
|
}
|
|||
|
|
*/ // Normal Way Forward End
|
|||
|
|
|
|||
|
|
// Follow Mode => directly drive motors with distance value
|
|||
|
|
int right_ir = irRead(IrRecVr,IrLedVr,'d');
|
|||
|
|
int left_ir = irRead(IrRecVl,IrLedVl,'d');
|
|||
|
|
|
|||
|
|
float right_ir_f = right_ir;
|
|||
|
|
float left_ir_f = left_ir;
|
|||
|
|
char f[10];
|
|||
|
|
|
|||
|
|
right_ir_f = right_ir_f / 100;
|
|||
|
|
left_ir_f = left_ir_f / 100;
|
|||
|
|
|
|||
|
|
Serial.print(" R IR: ");
|
|||
|
|
dtostrf(right_ir_f*100,6,3,f);
|
|||
|
|
Serial.print(f);
|
|||
|
|
|
|||
|
|
Serial.print(" L IR: ");
|
|||
|
|
dtostrf(left_ir_f*100,6,3,f);
|
|||
|
|
Serial.print(f);
|
|||
|
|
|
|||
|
|
rspeed = ( right_ir_f - 0.3 ) / 0.7;
|
|||
|
|
lspeed = ( left_ir_f - 0.3 ) / 0.7;
|
|||
|
|
|
|||
|
|
/*
|
|||
|
|
Serial.print(" R IR2: ");
|
|||
|
|
Serial.print(rspeed);
|
|||
|
|
Serial.print(" L IR2: ");
|
|||
|
|
Serial.print(lspeed);
|
|||
|
|
*/
|
|||
|
|
|
|||
|
|
// IR from -1 to 1
|
|||
|
|
rspeed = constrain(rspeed,-1,1);
|
|||
|
|
lspeed = constrain(lspeed,-1,1);
|
|||
|
|
|
|||
|
|
Serial.print(" - R MOT: ");
|
|||
|
|
dtostrf(rspeed*100,6,3,f);
|
|||
|
|
Serial.print(f);
|
|||
|
|
//Serial.print(rspeed * 100, DEC);
|
|||
|
|
Serial.print(" L MOT: ");
|
|||
|
|
dtostrf(lspeed*100,6,3,f);
|
|||
|
|
Serial.print(f);
|
|||
|
|
// Serial.print(lspeed * 100, DEC);
|
|||
|
|
|
|||
|
|
/* Serial.print(" l ");
|
|||
|
|
Serial.print(lspeed);
|
|||
|
|
Serial.print(" r ");
|
|||
|
|
Serial.println(rspeed);*/
|
|||
|
|
// spause = 500;
|
|||
|
|
// digitalWrite(ledPin,HIGH);
|
|||
|
|
delay(5);
|
|||
|
|
|
|||
|
|
// Disable Motors if requested
|
|||
|
|
if (motsta == false){
|
|||
|
|
lspeed = 0;
|
|||
|
|
rspeed = 0; };
|
|||
|
|
|
|||
|
|
lspeed = lspeed * 175;
|
|||
|
|
rspeed = rspeed * 175;
|
|||
|
|
|
|||
|
|
/*
|
|||
|
|
Serial.print(" - R MOT: ");
|
|||
|
|
Serial.print(rspeed);
|
|||
|
|
Serial.print(" L MOT: ");
|
|||
|
|
Serial.println(lspeed);
|
|||
|
|
*/
|
|||
|
|
|
|||
|
|
drive(lspeed,rspeed);
|
|||
|
|
Serial.println(" ");
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
/******************************************************************************
|
|||
|
|
/* 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;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
|
|||
|
|
if(protsta && zustand || mode == 'd'){
|
|||
|
|
|
|||
|
|
// for Debug
|
|||
|
|
/* 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);
|
|||
|
|
*/
|
|||
|
|
|
|||
|
|
if(mode == 'd'){
|
|||
|
|
//Serial.print(" irwert ");
|
|||
|
|
//Serial.print(irwert);
|
|||
|
|
irwert = map(irwert, 0, 220, 100, 0);
|
|||
|
|
constrain(irwert,0,100);
|
|||
|
|
//Serial.print(" dis ");
|
|||
|
|
//Serial.print(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
|