dinoi/follower_bot_formula
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

final

Browse Source
This commit is contained in:
reto
2021-02-22 11:00:58 +01:00
commit 2fd720ad5e
9 changed files with 866 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

5
.gitignore vendored Normal file
View File

@ -0,0 +1,5 @@
.pio
.vscode/.browse.c_cpp.db*
.vscode/c_cpp_properties.json
.vscode/launch.json
.vscode/ipch

67
.travis.yml Normal file
View File

@ -0,0 +1,67 @@
# Continuous Integration (CI) is the practice, in software
# engineering, of merging all developer working copies with a shared mainline
# several times a day < https://docs.platformio.org/page/ci/index.html >
#
# Documentation:
#
# * Travis CI Embedded Builds with PlatformIO
# < https://docs.travis-ci.com/user/integration/platformio/ >
#
# * PlatformIO integration with Travis CI
# < https://docs.platformio.org/page/ci/travis.html >
#
# * User Guide for `platformio ci` command
# < https://docs.platformio.org/page/userguide/cmd_ci.html >
#
#
# Please choose one of the following templates (proposed below) and uncomment
# it (remove "# " before each line) or use own configuration according to the
# Travis CI documentation (see above).
#
#
# Template #1: General project. Test it using existing `platformio.ini`.
#
# language: python
# python:
# - "2.7"
#
# sudo: false
# cache:
# directories:
# - "~/.platformio"
#
# install:
# - pip install -U platformio
# - platformio update
#
# script:
# - platformio run
#
# Template #2: The project is intended to be used as a library with examples.
#
# language: python
# python:
# - "2.7"
#
# sudo: false
# cache:
# directories:
# - "~/.platformio"
#
# env:
# - PLATFORMIO_CI_SRC=path/to/test/file.c
# - PLATFORMIO_CI_SRC=examples/file.ino
# - PLATFORMIO_CI_SRC=path/to/test/directory
#
# install:
# - pip install -U platformio
# - platformio update
#
# script:
# - platformio ci --lib="." --board=ID_1 --board=ID_2 --board=ID_N

7
.vscode/extensions.json vendored Normal file
View File

@ -0,0 +1,7 @@
{
// See http://go.microsoft.com/fwlink/?LinkId=827846
// for the documentation about the extensions.json format
"recommendations": [
"platformio.platformio-ide"
]
}

335
follower_bot_formula.ino_save Normal file
View File

@ -0,0 +1,335 @@
/******************************************************************************
// 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

39
include/README Normal file
View File

@ -0,0 +1,39 @@
This directory is intended for project header files.
A header file is a file containing C declarations and macro definitions
to be shared between several project source files. You request the use of a
header file in your project source file (C, C++, etc) located in `src` folder
by including it, with the C preprocessing directive `#include'.
```src/main.c
#include "header.h"
int main (void)
{
...
}
```
Including a header file produces the same results as copying the header file
into each source file that needs it. Such copying would be time-consuming
and error-prone. With a header file, the related declarations appear
in only one place. If they need to be changed, they can be changed in one
place, and programs that include the header file will automatically use the
new version when next recompiled. The header file eliminates the labor of
finding and changing all the copies as well as the risk that a failure to
find one copy will result in inconsistencies within a program.
In C, the usual convention is to give header files names that end with `.h'.
It is most portable to use only letters, digits, dashes, and underscores in
header file names, and at most one dot.
Read more about using header files in official GCC documentation:
* Include Syntax
* Include Operation
* Once-Only Headers
* Computed Includes
https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html

46
lib/README Normal file
View File

@ -0,0 +1,46 @@
This directory is intended for project specific (private) libraries.
PlatformIO will compile them to static libraries and link into executable file.
The source code of each library should be placed in a an own separate directory
("lib/your_library_name/[here are source files]").
For example, see a structure of the following two libraries `Foo` and `Bar`:
|--lib
| |
| |--Bar
| | |--docs
| | |--examples
| | |--src
| | |- Bar.c
| | |- Bar.h
| | |- library.json (optional, custom build options, etc) https://docs.platformio.org/page/librarymanager/config.html
| |
| |--Foo
| | |- Foo.c
| | |- Foo.h
| |
| |- README --> THIS FILE
|
|- platformio.ini
|--src
|- main.c
and a contents of `src/main.c`:
```
#include <Foo.h>
#include <Bar.h>
int main (void)
{
...
}
```
PlatformIO Library Dependency Finder will find automatically dependent
libraries scanning project source files.
More information about PlatformIO Library Dependency Finder
- https://docs.platformio.org/page/librarymanager/ldf.html

15
platformio.ini Normal file
View File

@ -0,0 +1,15 @@
; PlatformIO Project Configuration File
;
; Build options: build flags, source filter
; Upload options: custom upload port, speed and extra flags
; Library options: dependencies, extra library storages
; Advanced options: extra scripting
;
; Please visit documentation for the other options and examples
; https://docs.platformio.org/page/projectconf.html
[env:diecimilaatmega328]
platform = atmelavr
board = diecimilaatmega328
framework = arduino
monitor_speed = 57600

339
src/main.cpp Normal file
View File

@ -0,0 +1,339 @@
#include <Arduino.h>
/******************************************************************************
// 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 200 // 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);
}
/******************************************************************************
/* 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
/******************************************************************************
/* 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.6;
lspeed = ( left_ir_f - 0.3 ) / 0.6;
/*
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(" ");
}

13
test/README Normal file
View File

@ -0,0 +1,13 @@
This directory is intended for PIO Unit Testing and project tests.
Unit Testing is a software testing method by which individual units of
source code, sets of one or more MCU program modules together with associated
control data, usage procedures, and operating procedures, are tested to
determine whether they are fit for use. Unit testing finds problems early
in the development cycle.
More information about PIO Unit Testing:
- https://docs.platformio.org/page/plus/unit-testing.html
1