Commit 766f3d6d authored by Jens Noack's avatar Jens Noack

Added lib and Doxyfile

parent bbbcd70e
Pipeline #20 failed with stage
in 47 seconds
This diff is collapsed.
This diff is collapsed.
#include <CodeRacer.h> #include <CodeRacer.h>
//----- Werte für den Ultraschallsensor ----- //----- settings for the ultrasonic sensor -----
#define US_STOP_ABSTAND_CM 15 // Wenn der gemessene Abstand kleiner ist, hält der CodeRacer an #define US_STOP_ABSTAND_CM 20 // if distance goes below that - stop the racer
//----- Variablen, die wir brauchen um uns Werte zu merken ---- //----- {CODES}RACER API -> online: https://doc.itsblue.de/Fenoglio/coderacer/Doku/Doxygen/html/
long abstand_vorn_cm, abstand_links_cm, abstand_rechts_cm; //-- Some main higher level methods listed below
// void CodeRacer::stop_driving () Stops the racer and sets status LEDs
// void CodeRacer::drive_forward () Sets the speed and the directions of both drives so that it will move forward.
// void CodeRacer::drive_backward () Sets the speed and the directions of both drives so that it will move backward.
// void CodeRacer::turn_left () Will turn the racer to the left for the internally stored time in ms and with the internally stored speed.
// void CodeRacer::turn_right () Will turn the racer to the right for the internally stored time in ms and with the internally stored speed.
// void CodeRacer::start_stop_at_min_distance () Enables to stopp the racer if during a distance measurement the measured distance is smaller then the internally stored minimal distance.
// void CodeRacer::stop_stop_at_min_distance () Disables to stopp the racer if during a distance measurement the measured distance is smaller then the specified minimal distance.
// bool CodeRacer::start_stop () This will return if the codracer is in active mode or not.
// void CodeRacer::servo_set_to_right () Drives the servo to the postion that is defined by #servo_right_pos.
// void CodeRacer::servo_set_to_left () Drives the servo to the postion that is defined by #servo_left_pos.
// void CodeRacer::servo_set_to_center () Drives the servo to the postion that is defined by #servo_center_pos.
// uint8_t CodeRacer::servo_position () Get the actual position of the servo.
// unsigned long CodeRacer::usonic_measure_cm () Measures the distance to the next object in front of the ultra sonic sensor in cm.
//
// ... there are much more ... read the online API for more details.
//----- variables we need
unsigned long distance_cm = 0;
//---- construct the coderacer object
CodeRacer coderacer; CodeRacer coderacer;
// Das kann der coderacer:
// coderacer.start_stop() Abfragen ob der Racer fahren soll oder nicht ... wenn er fahren soll kommt der Wert 'true' zurück, wenn er stopppen soll der Wert 'false'
// coderacer.servo_schwenk() Abstandssensor hin und her verstellen
// coderacer.abstand_messen(); Abstand messen - es kommt ein Wert in cm zurück
// coderacer.servo_mitte(); nach vorn "schauen"
// coderacer.servo_rechts(); nach rechts "schauen"
// coderacer.servo_links(); nach links "schauen"
// coderacer.links(); nach links drehen
// coderacer.rechts(); nach rechts drehen
// coderacer.vorwaerts(); Vorwaerts fahren
// coderacer.anhalten(); Racer anhalten
//---- Hier startet der Code zum Einstellen aller wichtigen Dinge. Setup() wird einmal ausgeführt. ----
void setup() {
// Monitor
Serial.begin(115200); // Serial Monitor aktivieren. Mit dem Monitor kann man sich Werte und Meldungen anzeigen lassen.
// CodeRacer initialisieren //---- set up code - executed ones
void setup() {
// start serial monitor
Serial.begin(115200);
// initialize the coderacer
coderacer.begin(); coderacer.begin();
// enable fun stuff
// nach links und rechts schauen ... :-) coderacer.coderacer_fun_enabled = true;
coderacer.servo_links(); // look to the left, to the right and to center... :-)
delay(10); coderacer.servo_set_to_left();
coderacer.servo_rechts(); delay(100);
delay(10); coderacer.servo_set_to_right();
coderacer.servo_mitte(); delay(100);
coderacer.servo_set_to_center();
delay(100);
} }
//---- Hier startet unsere endlose Schleife - die immer wieder von vorn angefangen wird, wenn wir am Ende angekommen sind. Da ist unser "Fahr"Code drin, der den CodeRacer steuert //---- 'endless' loop
void loop() { void loop() {
// Abstand messen -> dort wo der coderacer gerade "hinschaut". Der gemessene Abstand ist in abstand_vorn_cm gespeichert // check if the racer was started (button was toggled to coderacer active state
abstand_vorn_cm = coderacer.abstand_messen();
// Abfragen ob der Racer fahren soll oder nicht ...
if(true == coderacer.start_stop()){ if(true == coderacer.start_stop()){
Serial.print("Speed of right side drive: ");
Serial.println(coderacer.drive_right_speed());
Serial.print("Speed of left side drive: ");
Serial.println(coderacer.drive_left_speed());
// measure the distance - at the position of the servo
distance_cm = coderacer.usonic_measure_cm();
// Abstandssensor verstellen ... coderacer.start_stop_at_min_distance(US_STOP_ABSTAND_CM);
coderacer.servo_schwenk(); while(!coderacer.stopped_at_min_distance()){
Serial.print("Distanc in cm: ");
Serial.println(distance_cm);
if(distance_cm > 50){
coderacer.drive_forward();
coderacer.servo_sweep();
}
else if(distance_cm > 40){
coderacer.turn_right();
}
else if(distance_cm > 30){
coderacer.turn_left();
}
else {
coderacer.drive_backward();
}
// hier kommt Euer Code zu steuern des coderacers hinein ... // measure the distance - at the position of the servo
distance_cm = coderacer.usonic_measure_cm();
}
Serial.println("***** STOPPED ***** ");
Serial.print("Measured stop distanc of cm: ");
Serial.println(distance_cm);
Serial.print("Measured at servo position of: ");
Serial.println(coderacer.servo_position());
coderacer.set_inactive();
} }
} }
......
CodeRacer KEYWORD1
begin KEYWORD2
servo_einstellungen KEYWORD2
motor_einstellungen KEYWORD2
anhalten KEYWORD2
normal_tempo KEYWORD2
vorwaerts KEYWORD2
links KEYWORD2
rechts KEYWORD2
servo_rechts KEYWORD2
servo_links KEYWORD2
servo_mitte KEYWORD2
abstand_messen KEYWORD2
servo_schwenk KEYWORD2
start_stop KEYWORD2
\ No newline at end of file
# Servo Library for ESP32
This library attempts to faithfully replicate the semantics of the
Arduino Servo library (see http://www.arduino.cc/en/Reference/Servo)
for the ESP32, with two (optional) additions. The two new functions
expose the ability of the ESP32 PWM timers to vary timer width.
## License
Copyright (c) 2017 John K. Bennett. All right reserved.
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
## Library Description:
```
  Servo - Class for manipulating servo motors connected to ESP32 pins.
int attach(pin ) - Attaches the given GPIO pin to the next free channel
(channels that have previously been detached are used first),
returns channel number or 0 if failure. All pin numbers are allowed,
but only pins 2,4,12-19,21-23,25-27,32-33 are recommended.
int attach(pin, min, max ) - Attaches to a pin setting min and max
values in microseconds; enforced minimum min is 500, enforced max
is 2500. Other semantics are the same as attach().
void write () - Sets the servo angle in degrees; a value below 500 is
treated as a value in degrees (0 to 180). These limit are enforced,
i.e., values are constrained as follows:
Value Becomes
----- -------
< 0 0
0 - 180 value (treated as degrees)
181 - 499 180
500 - (min-1) min
min-max (from attach or default) value (treated as microseconds)
(max+1) - 2500 max
void writeMicroseconds() - Sets the servo pulse width in microseconds.
min and max are enforced (see above).
int read() - Gets the last written servo pulse width as an angle between 0 and 180.
int readMicroseconds() - Gets the last written servo pulse width in microseconds.
bool attached() - Returns true if this servo instance is attached to a pin.
void detach() - Stops an the attached servo, frees the attached pin, and frees
its channel for reuse.
```
### **New ESP32-specific functions**
```
  setTimerWidth(value) - Sets the PWM timer width (must be 16-20) (ESP32 ONLY);
as a side effect, the pulse width is recomputed.
  int readTimerWidth() - Gets the PWM timer width (ESP32 ONLY)
```
### Useful Defaults:
default min pulse width for attach(): 544us
default max pulse width for attach(): 2400us
default timer width 16 (if timer width is not set)
default pulse width 1500us (servos are initialized with this value)
MINIMUM pulse with: 500us
MAXIMUM pulse with: 2500us
MAXIMUM number of servos: 16 (this is the number of PWM channels in the ESP32)
/*
Controlling a servo position using a potentiometer (variable resistor)
by Michal Rinott <http://people.interaction-ivrea.it/m.rinott>
modified on 8 Nov 2013
by Scott Fitzgerald
modified for the ESP32 on March 2017
by John Bennett
see http://www.arduino.cc/en/Tutorial/Knob for a description of the original code
* Different servos require different pulse widths to vary servo angle, but the range is
* an approximately 500-2500 microsecond pulse every 20ms (50Hz). In general, hobbyist servos
* sweep 180 degrees, so the lowest number in the published range for a particular servo
* represents an angle of 0 degrees, the middle of the range represents 90 degrees, and the top
* of the range represents 180 degrees. So for example, if the range is 1000us to 2000us,
* 1000us would equal an angle of 0, 1500us would equal 90 degrees, and 2000us would equal 1800
* degrees.
*
* Circuit: (using an ESP32 Thing from Sparkfun)
* Servo motors have three wires: power, ground, and signal. The power wire is typically red,
* the ground wire is typically black or brown, and the signal wire is typically yellow,
* orange or white. Since the ESP32 can supply limited current at only 3.3V, and servos draw
* considerable power, we will connect servo power to the VBat pin of the ESP32 (located
* near the USB connector). THIS IS ONLY APPROPRIATE FOR SMALL SERVOS.
*
* We could also connect servo power to a separate external
* power source (as long as we connect all of the grounds (ESP32, servo, and external power).
* In this example, we just connect ESP32 ground to servo ground. The servo signal pins
* connect to any available GPIO pins on the ESP32 (in this example, we use pin 18.
*
* In this example, we assume a Tower Pro SG90 small servo connected to VBat.
* The published min and max for this servo are 500 and 2400, respectively.
* These values actually drive the servos a little past 0 and 180, so
* if you are particular, adjust the min and max values to match your needs.
*/
// Include the ESP32 Arduino Servo Library instead of the original Arduino Servo Library
#include <ESP32Servo.h>
Servo myservo; // create servo object to control a servo
// Possible PWM GPIO pins on the ESP32: 0(used by on-board button),2,4,5(used by on-board LED),12-19,21-23,25-27,32-33
int servoPin = 18; // GPIO pin used to connect the servo control (digital out)
// Possible ADC pins on the ESP32: 0,2,4,12-15,32-39; 34-39 are recommended for analog input
int potPin = 34; // GPIO pin used to connect the potentiometer (analog in)
int ADC_Max = 4096; // This is the default ADC max value on the ESP32 (12 bit ADC width);
// this width can be set (in low-level oode) from 9-12 bits, for a
// a range of max values of 512-4096
int val; // variable to read the value from the analog pin
void setup()
{
myservo.attach(servoPin, 500, 2400); // attaches the servo on pin 18 to the servo object
// using SG90 servo min/max of 500us and 2400us
// for MG995 large servo, use 1000us and 2000us,
// which are the defaults, so this line could be
// "myservo.attach(servoPin);"
}
void loop() {
val = analogRead(potPin); // read the value of the potentiometer (value between 0 and 1023)
val = map(val, 0, ADC_Max, 0, 180); // scale it to use it with the servo (value between 0 and 180)
myservo.write(val); // set the servo position according to the scaled value
delay(200); // wait for the servo to get there
}
/*
* ESP32 Servo Example Using Arduino ESP32 Servo Library
* John K. Bennett
* March, 2017
*
* This sketch uses the Arduino ESP32 Servo Library to sweep 4 servos in sequence.
*
* Different servos require different pulse widths to vary servo angle, but the range is
* an approximately 500-2500 microsecond pulse every 20ms (50Hz). In general, hobbyist servos
* sweep 180 degrees, so the lowest number in the published range for a particular servo
* represents an angle of 0 degrees, the middle of the range represents 90 degrees, and the top
* of the range represents 180 degrees. So for example, if the range is 1000us to 2000us,
* 1000us would equal an angle of 0, 1500us would equal 90 degrees, and 2000us would equal 1800
* degrees.
*
* Circuit:
* Servo motors have three wires: power, ground, and signal. The power wire is typically red,
* the ground wire is typically black or brown, and the signal wire is typically yellow,
* orange or white. Since the ESP32 can supply limited current at only 3.3V, and servos draw
* considerable power, we will connect servo power to the VBat pin of the ESP32 (located
* near the USB connector). THIS IS ONLY APPROPRIATE FOR SMALL SERVOS.
*
* We could also connect servo power to a separate external
* power source (as long as we connect all of the grounds (ESP32, servo, and external power).
* In this example, we just connect ESP32 ground to servo ground. The servo signal pins
* connect to any available GPIO pins on the ESP32 (in this example, we use pins
* 22, 19, 23, & 18).
*
* In this example, we assume four Tower Pro SG90 small servos.
* The published min and max for this servo are 500 and 2400, respectively.
* These values actually drive the servos a little past 0 and 180, so
* if you are particular, adjust the min and max values to match your needs.
* Experimentally, 550 and 2350 are pretty close to 0 and 180.
*/
#include <ESP32Servo.h>
// create four servo objects
Servo servo1;
Servo servo2;
Servo servo3;
Servo servo4;
// Published values for SG90 servos; adjust if needed
int minUs = 500;
int maxUs = 2400;
// These are all GPIO pins on the ESP32
// Recommended pins include 2,4,12-19,21-23,25-27,32-33
int servo1Pin = 18;
int servo2Pin = 19;
int servo3Pin = 22;
int servo4Pin = 23;
int pos = 0; // position in degrees
void setup()
{
servo1.attach(servo1Pin, minUs, maxUs);
servo2.attach(servo2Pin, minUs, maxUs);
servo3.attach(servo3Pin, minUs, maxUs);
servo4.attach(servo4Pin, minUs, maxUs);
}
void loop() {
for (pos = 0; pos <= 180; pos += 1) { // sweep from 0 degrees to 180 degrees
// in steps of 1 degree
servo1.write(pos);
delay(20); // waits 20ms for the servo to reach the position
}
for (pos = 180; pos >= 0; pos -= 1) { // sweep from 180 degrees to 0 degrees
servo1.write(pos);
delay(20);
}
for (pos = 0; pos <= 180; pos += 1) { // sweep from 0 degrees to 180 degrees
// in steps of 1 degree
servo2.write(pos);
delay(20); // waits 20ms for the servo to reach the position
}
for (pos = 180; pos >= 0; pos -= 1) { // sweep from 180 degrees to 0 degrees
servo2.write(pos);
delay(20);
}
for (pos = 0; pos <= 180; pos += 1) { // sweep from 0 degrees to 180 degrees
// in steps of 1 degree
servo3.write(pos);
delay(20); // waits 20ms for the servo to reach the position
}
for (pos = 180; pos >= 0; pos -= 1) { // sweep from 180 degrees to 0 degrees
servo3.write(pos);
delay(20);
}
for (pos = 0; pos <= 180; pos += 1) { // sweep from 0 degrees to 180 degrees
// in steps of 1 degree
servo4.write(pos);
delay(20); // waits 20ms for the servo to reach the position
}
for (pos = 180; pos >= 0; pos -= 1) { // sweep from 180 degrees to 0 degrees
servo4.write(pos);
delay(20);
}
}
/*
* ESP32 Servo Example
* John K. Bennett
* March, 2017
*
* This sketch uses low-level ESP32 PWM functionality to sweep 4 servos in sequence.
* It does NOT use the ESP32_Servo library for Arduino.
*
* The ESP32 supports 16 hardware LED PWM channels that are intended
* to be used for LED brightness control. The low level ESP32 code allows us to set the
* PWM frequency and bit-depth, and then control them by setting bits in the relevant control
* register. The core files esp32-hal-ledc.* provides helper functions to make this set up
* straightforward.
*
* Different servos require different pulse widths to vary servo angle, but the range is
* an approximately 500-2500 microsecond pulse every 20ms (50Hz). In general, hobbyist servos
* sweep 180 degrees, so the lowest number in the published range for a particular servo
* represents an angle of 0 degrees, the middle of the range represents 90 degrees, and the top
* of the range represents 180 degrees. So for example, if the range is 1000us to 2000us,
* 1000us would equal an angle of 0, 1500us would equal 90 degrees, and 2000us would equal 1800
* degrees.
*
* The ESP32 PWM timers allow us to set the timer width (max 20 bits). Thus
* the timer "tick" length is (pulse_period/2**timer_width), and the equation for pulse_high_width
* (the portion of cycle (20ms in our case) that the signal is high) becomes:
*
* pulse_high_width = count * tick_length
* = count * (pulse_period/2**timer_width)
*
* and count = (pulse_high_width / (pulse_period/2**timer_width))
*
* For example, if we want a 1500us pulse_high_width, we set pulse_period to 20ms (20000us)
* (this value is set in the ledcSetup call), and count (used in the ledcWrite call) to
* 1500/(20000/65655), or 4924. This is the value we write to the timer in the ledcWrite call.
*
* As a concrete example, suppose we want to repeatedly sweep four Tower Pro SG90 servos
* from 0 to 180 degrees. The published pulse width range for the SG90 is 500-2400us. Thus,
* we should vary the count used in ledcWrite from 1638 to 7864.
*
* Circuit:
* Servo motors have three wires: power, ground, and signal. The power wire is typically red,
* the ground wire is typically black or brown, and the signal wire is typically yellow,
* orange or white. Since the ESP32 can supply limited current at only 3.3V, and servos draw
* considerable power, we will connect servo power to the VBat pin of the ESP32 (located
* near the USB connector). THIS IS ONLY APPROPRIATE FOR SMALL SERVOS.
*
* We could also connect servo power to a separate external
* power source (as long as we connect all of the grounds (ESP32, servo, and external power).
* In this example, we just connect ESP32 ground to servo ground. The servo signal pins
* connect to any available GPIO pins on the ESP32 (in this example, we use pins
* 22, 19, 23, & 18).
*
* In this example, we assume four Tower Pro SG90 small servos.
* The published min and max for this servo are 500 and 2400, respectively.
* These values actually drive the servos a little past 0 and 180, so
* if you are particular, adjust the min and max values to match your needs.
* Experimentally, 550us and 2350us are pretty close to 0 and 180.
*
* This code was inspired by a post on Hackaday by Elliot Williams.
*/
// Values for TowerPro SG90 small servos; adjust if needed
#define COUNT_LOW 1638
#define COUNT_HIGH 7864
#define TIMER_WIDTH 16
#include "esp32-hal-ledc.h"
void setup() {
ledcSetup(1, 50, TIMER_WIDTH); // channel 1, 50 Hz, 16-bit width
ledcAttachPin(22, 1); // GPIO 22 assigned to channel 1
ledcSetup(2, 50, TIMER_WIDTH); // channel 2, 50 Hz, 16-bit width
ledcAttachPin(19, 2); // GPIO 19 assigned to channel 2
ledcSetup(3, 50, TIMER_WIDTH); // channel 3, 50 Hz, 16-bit width
ledcAttachPin(23, 3); // GPIO 23 assigned to channel 3
ledcSetup(4, 50, TIMER_WIDTH); // channel 4, 50 Hz, 16-bit width
ledcAttachPin(18, 4); // GPIO 18 assigned to channel 4
}
void loop() {
for (int i=COUNT_LOW ; i < COUNT_HIGH ; i=i+100)
{
ledcWrite(1, i); // sweep servo 1
delay(200);
}
for (int i=COUNT_LOW ; i < COUNT_HIGH ; i=i+100)
{
ledcWrite(2, i); // sweep servo 2
delay(200);
}
for (int i=COUNT_LOW ; i < COUNT_HIGH ; i=i+100)
{
ledcWrite(3, i); // sweep the servo
delay(200);
}
for (int i=COUNT_LOW ; i < COUNT_HIGH ; i=i+100)
{
ledcWrite(4, i); // sweep the servo
delay(200);
}
}
/* Sweep
by BARRAGAN <http://barraganstudio.com>
This example code is in the public domain.
modified 8 Nov 2013
by Scott Fitzgerald
modified for the ESP32 on March 2017
by John Bennett
see http://www.arduino.cc/en/Tutorial/Sweep for a description of the original code
* Different servos require different pulse widths to vary servo angle, but the range is
* an approximately 500-2500 microsecond pulse every 20ms (50Hz). In general, hobbyist servos
* sweep 180 degrees, so the lowest number in the published range for a particular servo
* represents an angle of 0 degrees, the middle of the range represents 90 degrees, and the top
* of the range represents 180 degrees. So for example, if the range is 1000us to 2000us,
* 1000us would equal an angle of 0, 1500us would equal 90 degrees, and 2000us would equal 1800
* degrees.
*
* Circuit: (using an ESP32 Thing from Sparkfun)
* Servo motors have three wires: power, ground, and signal. The power wire is typically red,
* the ground wire is typically black or brown, and the signal wire is typically yellow,
* orange or white. Since the ESP32 can supply limited current at only 3.3V, and servos draw
* considerable power, we will connect servo power to the VBat pin of the ESP32 (located
* near the USB connector). THIS IS ONLY APPROPRIATE FOR SMALL SERVOS.
*
* We could also connect servo power to a separate external