1808 lines
68 KiB
C++
1808 lines
68 KiB
C++
// the compiler switch for an ESP8266 is looking like this: #elif defined(ARDUINO_ARCH_ESP8266)
|
|
#include "CodeRacer_MKII.h"
|
|
|
|
using namespace std;
|
|
|
|
static void _count_steps_left();
|
|
static void _count_steps_right();
|
|
|
|
static volatile unsigned int left_step_counter=0;
|
|
static volatile unsigned int left_step_stopcount=0;
|
|
static volatile unsigned int right_step_counter=0;
|
|
static volatile unsigned int right_step_stopcount=0;
|
|
static volatile bool syncstop= true;
|
|
static volatile bool _drive;
|
|
static volatile float timebeforeleft= 0;
|
|
static volatile float timebeforeright= 0;
|
|
static volatile float timeafterleft= 0;
|
|
static volatile float timeafterright= 0;
|
|
static volatile bool obstacle_stop= false;
|
|
static volatile bool obstacle_left_side= false;
|
|
static volatile bool obstacle_right_side= false;
|
|
static volatile bool coderracer_activ = false;
|
|
static volatile bool right_button_pressed =false;
|
|
static volatile unsigned int right_button_count=0;
|
|
static volatile unsigned int driven_distance= 0;
|
|
static volatile unsigned long left_button_last_pressed_at_ms = millis();
|
|
static volatile unsigned long right_button_last_pressed_at_ms = millis();
|
|
static String debug_message= "";
|
|
|
|
|
|
/** @brief CodeRace constructor without pins. All pins settings taken from the coderacer header file
|
|
* @return nothing
|
|
*/
|
|
CodeRacerMKII::CodeRacerMKII()
|
|
{
|
|
_button_pin_left = H_BUTTON_PIN_LEFT;
|
|
_button_pin_right = H_BUTTON_PIN_RIGHT;
|
|
_servo_pin = H_SERVO_PIN;
|
|
_barrier_pin_left = H_LEFT_BARRIER_PIN;
|
|
_barrier_pin_right = H_RIGHT_BARRIER_PIN;
|
|
_infrared_sensor_left = H_INFRARED_SENSOR_LEFT;
|
|
_infrared_sensor_right = H_INFRARED_SENSOR_RIGHT;
|
|
_us_trigger_pin = H_US_TRIG_PIN;
|
|
_us_echo_pin = H_US_ECHO_PIN;
|
|
_drive_left_frwd_bkwrd_pin = H_DRIVE_LEFT_FWRD_BKWRD_PIN;
|
|
_drive_left_enable_pin = H_DRIVE_LEFT_ENABLE_PIN;
|
|
_drive_right_frwd_bkwrd_pin = H_DRIVE_RIGHT_FWRD_BKWRD_PIN;
|
|
_drive_right_enable_pin = H_DRIVE_RIGHT_ENABLE_PIN;
|
|
_led_frwd_pin = H_LED_FRWD_PIN;
|
|
_led_stop_pin = H_LED_STOP_PIN;
|
|
_led_left_pin = H_LED_LEFT_PIN;
|
|
_led_right_pin = H_LED_RIGHT_PIN;
|
|
_led_blue_pin = H_LED_BLUE_PIN;
|
|
_led_white_pin = H_LED_WHITE_PIN;
|
|
_switch_1 = H_SWITCH_1;
|
|
_switch_2 = H_SWITCH_2;
|
|
_switch_3 = H_SWITCH_3;
|
|
_switch_4= H_SWITCH_4;
|
|
}
|
|
|
|
/** @brief CodeRace constructor with pins.This will overwrite the default settings taken from the header file.
|
|
* @param button_pin_left Pin the left button is connected at
|
|
* @param button_pin_right Pin the right button is connected at
|
|
* @param servo_pin Pin the servo drive is connected at
|
|
* @param left_barrier_pin Pin the left photoelectric barrier is connected at
|
|
* @param right_barrier_pin Pin the right photoelectric barrier is connected at
|
|
* @param infrared_sensor_left Pin the left infrared sensor is connected at
|
|
* @param infrared_sensor_right Pin the right infrared sensor is connected at
|
|
* @param us_trigger_pin Pin the trigger signal of the ultrasonic sensor is connected at
|
|
* @param us_echo_pin Pin the echo signal of the ultrasonic sensor is connected at
|
|
* @param drive_left_frwd_bkwrd_pin Pin the forward/ backward pin of the left side drive device driver is connected at
|
|
* @param drive_left_enable_pin Pin the enable pin of the left side drive device driver is connected at
|
|
* @param drive_right_frwd_bkwrd_pin Pin the forward/ backward pin of the right side drive device driver is connected at
|
|
* @param drive_right_enable_pin Pin the enable pin of the right side drive device driver is connected at
|
|
* @param led_frwd_pin Pin the led that signals forward direction is connected at
|
|
* @param led_stop_pin Pin the led that signals that the drives are stopped is connected at
|
|
* @param led_left_pin Pin the led that signals left side direction is connected at
|
|
* @param led_right_pin Pin the led that signals right side direction is connected at
|
|
* @param led_blue_pin Pin the blue led is connected at
|
|
* @param led_white_pin Pin the white led is connected at
|
|
@param switch_1 Pin the switch titled '1' is connected at
|
|
@param switch_2 Pin the switch titled '2' is connected at
|
|
@param switch_3 Pin the switch titled '3' is connected at
|
|
@param switch_4 Pin the switch titled '4' is connected at
|
|
* @return nothing
|
|
*/
|
|
CodeRacerMKII::CodeRacerMKII(uint8_t button_pin_left , uint8_t button_pin_right, uint8_t servo_pin,
|
|
uint8_t left_barrier_pin, uint8_t right_barrier_pin, uint8_t infrared_sensor_left, uint8_t infrared_sensor_right,
|
|
uint8_t us_trigger_pin, uint8_t us_echo_pin,
|
|
uint8_t drive_left_frwd_bkwrd_pin, uint8_t drive_left_enable_pin,
|
|
uint8_t drive_right_frwd_bkwrd_pin, uint8_t drive_right_enable_pin,
|
|
uint8_t led_frwd_pin, uint8_t led_stop_pin, uint8_t led_left_pin, uint8_t led_right_pin, uint8_t led_blue_pin, uint8_t led_white_pin, uint8_t switch_1,
|
|
uint8_t switch_2, uint8_t switch_3, uint8_t switch_4)
|
|
{
|
|
_button_pin_left = button_pin_left;
|
|
_button_pin_right = button_pin_right;
|
|
_servo_pin = servo_pin;
|
|
_barrier_pin_left = left_barrier_pin;
|
|
_barrier_pin_right = right_barrier_pin;
|
|
_infrared_sensor_left = infrared_sensor_left;
|
|
_infrared_sensor_right = infrared_sensor_right;
|
|
_us_trigger_pin = us_trigger_pin;
|
|
_us_echo_pin = us_echo_pin;
|
|
_drive_left_frwd_bkwrd_pin = drive_left_frwd_bkwrd_pin;
|
|
_drive_left_enable_pin = drive_left_enable_pin;
|
|
_drive_right_frwd_bkwrd_pin = drive_right_frwd_bkwrd_pin;
|
|
_drive_right_enable_pin = drive_right_enable_pin;
|
|
_led_frwd_pin = led_frwd_pin;
|
|
_led_stop_pin = led_stop_pin;
|
|
_led_left_pin = led_left_pin;
|
|
_led_right_pin = led_right_pin;
|
|
_led_blue_pin = led_blue_pin;
|
|
_led_white_pin= led_white_pin;
|
|
_switch_1= switch_1;
|
|
_switch_2= switch_2;
|
|
_switch_3= switch_3;
|
|
_switch_4= switch_4;
|
|
|
|
}
|
|
|
|
/** @brief Initialisation of all attributes and settings of the coderacer. Defaults are taken from the header file.
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::begin() {
|
|
|
|
// init of variables and objects
|
|
_bluetoothcreated = false;
|
|
_bt_stopOnLostConnection_timeout_ms = 0;
|
|
_bt_lastmessagereceived = millis();
|
|
_bt_ignoremsgs.clear();
|
|
_bt_onlymsgs.clear();
|
|
|
|
_servo_dummy = new Servo(); // the dummy is needed so far to avoid conflicts with analog write
|
|
_servo = new Servo();
|
|
servo_center_pos = H_SERVO_CENTER_POS;
|
|
servo_left_pos = H_SERVO_LEFT_POS;
|
|
servo_right_pos = H_SERVO_RIGHT_POS;
|
|
servo_sweep_left_pos = H_SERVO_SWEEP_LEFT_POS;
|
|
servo_sweep_right_pos = H_SERVO_SWEEP_RIGHT_POS;
|
|
_servo_position = servo_center_pos;
|
|
_servo_sweep_step = SERVO_SWEEP_TO_LEFT_STEP;
|
|
_servo_sweep_step_left = SERVO_SWEEP_TO_LEFT_STEP;
|
|
_servo_sweep_step_right = SERVO_SWEEP_TO_RIGHT_STEP;
|
|
_servo_position_set_at_ms = millis();
|
|
_servo_position_eta_in_ms = 0;
|
|
|
|
NVS.begin();
|
|
_get_calibration_factor();
|
|
_drive_left_speed = H_DRIVE_LEFT_SPEED;
|
|
_drive_right_speed = H_DRIVE_RIGHT_SPEED;
|
|
_turn_left_for_ms = H_RACER_TURN_LEFT_FOR_MS;
|
|
_turn_right_for_ms = H_RACER_TURN_RIGHT_FOR_MS;
|
|
|
|
coderracer_activ = false;
|
|
_coderracer_activ = true;
|
|
_drive = false;
|
|
_drive_set_at_ms = millis();
|
|
_servo_sweep = false;
|
|
|
|
_last_led_switched_at_ms = millis();
|
|
_last_led_on = 0;
|
|
_led_count = 6;
|
|
right_button_pressed = 0;
|
|
|
|
_servo_look_around_at_ms = millis() + random(FUN_MIN_PAUSE_MS, FUN_MAX_PAUSE_MS);
|
|
|
|
_usonic_stop_distance_cm = H_US_STOP_DISTANCE_CM;
|
|
usonic_set_stop_distance_cm(_usonic_stop_distance_cm);
|
|
_coderacer_stopped_at_min_distance = false;
|
|
|
|
// Ultrasonic sensor
|
|
pinMode(_us_trigger_pin, OUTPUT);
|
|
pinMode(_us_echo_pin, INPUT);
|
|
|
|
// Servo drive
|
|
_servo->attach(_servo_pin);
|
|
|
|
// PE barriers
|
|
pinMode(_barrier_pin_left, INPUT_PULLUP);
|
|
attachInterrupt(digitalPinToInterrupt(_barrier_pin_left), _count_steps_left, FALLING);
|
|
pinMode(_barrier_pin_right, INPUT_PULLUP);
|
|
attachInterrupt(digitalPinToInterrupt(_barrier_pin_right), _count_steps_right, FALLING);
|
|
|
|
// Infrared sensors
|
|
pinMode(_infrared_sensor_left, INPUT);
|
|
pinMode(_infrared_sensor_right, INPUT_PULLDOWN);
|
|
attachInterrupt(digitalPinToInterrupt(_infrared_sensor_left), _stop_obstacle_left, CHANGE);
|
|
attachInterrupt(digitalPinToInterrupt(_infrared_sensor_right), _stop_obstacle_right, CHANGE);
|
|
|
|
// Switches
|
|
pinMode(H_SWITCH_1, INPUT_PULLUP);
|
|
pinMode(H_SWITCH_1, INPUT_PULLUP);
|
|
pinMode(H_SWITCH_3, INPUT_PULLUP);
|
|
pinMode(H_SWITCH_4, INPUT_PULLUP);
|
|
|
|
|
|
// Left drive
|
|
pinMode(_drive_left_frwd_bkwrd_pin, OUTPUT);
|
|
set_drive_left_state(DRIVESTOP);
|
|
ledcSetup(DRIVE_PWM_LEFT_CHANNEL, 100, 8); // channel , 50 Hz, 8-bit width
|
|
ledcAttachPin(_drive_left_enable_pin, DRIVE_PWM_LEFT_CHANNEL); // connect left drive enable with PWM channel
|
|
|
|
// Right drive
|
|
pinMode(_drive_right_frwd_bkwrd_pin, OUTPUT);
|
|
set_drive_right_state(DRIVESTOP);
|
|
ledcSetup(DRIVE_PWM_RIGHT_CHANNEL, 100, 8); // channel , 50 Hz, 8-bit width
|
|
ledcAttachPin(_drive_right_enable_pin, DRIVE_PWM_RIGHT_CHANNEL); // connect right drive enable pin with PWM channel
|
|
|
|
// LEDs
|
|
pinMode(_led_frwd_pin, OUTPUT);
|
|
pinMode(_led_stop_pin, OUTPUT);
|
|
pinMode(_led_left_pin, OUTPUT);
|
|
pinMode(_led_right_pin, OUTPUT);
|
|
pinMode(_led_blue_pin, OUTPUT);
|
|
pinMode(_led_white_pin, OUTPUT);
|
|
set_leds_all_off();
|
|
|
|
// Button & -interrupt
|
|
left_button_last_pressed_at_ms = 0;
|
|
right_button_last_pressed_at_ms = 0;
|
|
pinMode(_button_pin_left, INPUT_PULLUP);
|
|
attachInterrupt(digitalPinToInterrupt(_button_pin_left), _set_button_state_left, FALLING);
|
|
pinMode(_button_pin_right, INPUT_PULLUP);
|
|
attachInterrupt(digitalPinToInterrupt(_button_pin_right), _set_button_state_right, FALLING);
|
|
|
|
// Random
|
|
//randomSeed(analogRead(0));
|
|
|
|
//fun stuff
|
|
coderacer_fun_enabled = true;
|
|
|
|
servo_set_to_center();
|
|
|
|
|
|
|
|
}
|
|
|
|
//**************************************
|
|
//*** Coderacer higher level methods ***
|
|
//**************************************
|
|
/** @defgroup higherlevel Higher level methods, setters and getters
|
|
* @{
|
|
*/
|
|
/** @defgroup higherlevelmeths Methods
|
|
* @{
|
|
*/
|
|
|
|
/** @brief Stops the racer.
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::stop_driving() {
|
|
_servo_sweep = false;
|
|
_drive=false;
|
|
set_drives_stop_left_right();
|
|
wifi_printf("Stopped.");
|
|
set_leds(LEDOFF, LEDOFF, LEDON, LEDOFF, LEDOFF, LEDOFF);
|
|
set_leds_all_off();
|
|
}
|
|
|
|
/** @brief Sets the speed and the directions of both drives so that it will move forward.
|
|
*
|
|
* The speed is taken from the header file or set by one of the methods defined in the lower level drive methods section
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::drive_forward()
|
|
{
|
|
drive_forward(_drive_left_speed, _drive_right_speed);
|
|
}
|
|
|
|
/** @brief Sets the speed as specified for both drives and the directions of both drives so that it will move forward.
|
|
*
|
|
* The specified speed values for both drives will be stored internaly so next time if you use e.g. drive_forward() exactly the here specified values will be taken.
|
|
* @param left_speed speed for the left side drive. 0<=speed<=255
|
|
* @param right_speed speed for the right side drive. 0<=speed<=255
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::drive_forward(uint8_t left_speed, uint8_t right_speed)
|
|
{
|
|
set_drives_states_left_right(DRIVEFRWD, DRIVEFRWD);
|
|
set_drives_speed_left_right(left_speed, right_speed);
|
|
wifi_printf("Driving forward...");
|
|
//set_leds(LEDOFF, LEDOFF, LEDOFF, LEDON, LEDOFF, LEDOFF);
|
|
_drive = true;
|
|
_drive_set_at_ms = millis();
|
|
}
|
|
/** @brief tells the Coderacer to drive forward for a specified amount of time and stop afterwards
|
|
* @param time time in ms the coderacer will drive
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::drive_forward_for_ms(unsigned long time ){
|
|
drive_forward();
|
|
wait_ms(time);
|
|
stop_driving();
|
|
}
|
|
|
|
/** @brief Sets the speed and the directions of both drives so that it will move backward.
|
|
*
|
|
* The speed is taken from the header file or set by one of the methods defined in the lower level drive methods section
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::drive_backward()
|
|
{
|
|
drive_backward(_drive_left_speed, _drive_right_speed);
|
|
}
|
|
|
|
/** @brief Sets the speed as specified for both drives and the directions of both drives so that it will move backward.
|
|
*
|
|
* The specified speed values for both drives will be stored internaly so next time if you use e.g. drive_backward() exactly the here specified values will be taken.
|
|
* @param left_speed speed for the left side drive. 0<=speed<=255
|
|
* @param right_speed speed for the right side drive. 0<=speed<=255
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::drive_backward(uint8_t left_speed, uint8_t right_speed)
|
|
{
|
|
set_drives_states_left_right(DRIVEBACK, DRIVEBACK);
|
|
set_drives_speed_left_right(left_speed, right_speed);
|
|
wifi_printf("Driving Backwards...");
|
|
//set_leds(LEDOFF, LEDOFF, LEDOFF, LEDOFF, LEDON, LEDOFF);
|
|
_drive = true;
|
|
_drive_set_at_ms = millis();
|
|
}
|
|
|
|
/** @brief tells the Coderacer to drive backward for a specified amount of time and stop afterwards
|
|
* @param time time in ms the coderacer will drive
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::drive_backward_for_ms(unsigned long time){
|
|
drive_backward();
|
|
wait_ms(time);
|
|
stop_driving();
|
|
}
|
|
|
|
/** @brief Will turn the racer to the left for the internally stroe time in ms and with the internally stored speed.
|
|
*
|
|
* The speed for both the left side and right side drive can be set by the methods described in the lower level drive section. The turn to left time can be set by thoose methods as well.
|
|
* The method is delayed until the racer has turned.
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::turn_left()
|
|
{
|
|
turn_left(_turn_left_for_ms, _drive_left_speed, _drive_right_speed);
|
|
}
|
|
|
|
/** @brief Will turn the racer to the left for a specified time in ms and with the internally stored speed.
|
|
*
|
|
* The specified duration of time is stored internally and will be used next time by e.g. turn_left()
|
|
* @param turn_for_ms duration of time in ms to turn the racer
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::turn_left(unsigned long turn_for_ms)
|
|
{
|
|
turn_left(turn_for_ms, _drive_left_speed, _drive_right_speed);
|
|
}
|
|
|
|
/** @brief Will turn the racer to the left for the specified time in ms and with the specified speed.
|
|
*
|
|
* The specified duration of time and the specified speeds are stored internally and will be used next time by e.g. turn_left()
|
|
* @param turn_for_ms duration of time in ms to turn the racer
|
|
* @param left_speed speed for the left side drive
|
|
* @param right_speed speed for the right side drive
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::turn_left(unsigned long turn_for_ms, uint8_t left_speed, uint8_t right_speed)
|
|
{
|
|
_drive = false;
|
|
//set_leds(LEDON, LEDOFF, LEDOFF, LEDOFF, LEDOFF, LEDOFF); // LEDs setzen
|
|
set_drives_states_left_right(DRIVEBACK, DRIVEFRWD);
|
|
|
|
set_drives_speed_left_right(left_speed, right_speed);
|
|
wifi_printf("Turning left! <p>");
|
|
// wait set delay for the timed turn
|
|
_turn_left_for_ms = turn_for_ms;
|
|
wait_ms(_turn_left_for_ms);
|
|
// stop drives again
|
|
stop_driving();
|
|
}
|
|
|
|
/** @brief Will turn the racer to the right for the internally stored time in ms and with the internally stored speed.
|
|
*
|
|
* The speed for both the left side and right side drive can be set by the methods described in the lower level drive section. The turn to right time can be set by thoose methods as well.
|
|
* The method is delayed until the racer has turned.
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::turn_right()
|
|
{
|
|
turn_right(_turn_right_for_ms, _drive_left_speed, _drive_right_speed);
|
|
}
|
|
|
|
/** @brief Will turn the racer to the right for the specified time in ms and with the internally stored speed.
|
|
*
|
|
* The specified duration of time is stored internally and will be used next time by e.g. turn_right()
|
|
* @param turn_for_ms duration of time in ms to turn the racer
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::turn_right(unsigned long turn_for_ms)
|
|
{
|
|
turn_right(turn_for_ms, _drive_left_speed, _drive_right_speed);
|
|
}
|
|
|
|
/** @brief Will turn the racer to the right for the specified time in ms and with the specified speed.
|
|
*
|
|
* The specified duration of time and the specified speeds are stored internally and will be used next time by e.g. turn_right()
|
|
* @param turn_for_ms duration of time in ms to turn the racer
|
|
* @param left_speed speed for the left side drive
|
|
* @param right_speed speed for the right side drive
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::turn_right(unsigned long turn_for_ms, uint8_t left_speed, uint8_t right_speed)
|
|
{
|
|
_drive = false;
|
|
//set_leds(LEDOFF, LEDON, LEDOFF, LEDOFF, LEDOFF, LEDOFF); // LEDs setzen
|
|
set_drives_states_left_right(DRIVEFRWD, DRIVEBACK);
|
|
set_drives_speed_left_right(left_speed, right_speed);
|
|
wifi_printf("Turning right! <p>");
|
|
// wait set delay for the timed turn
|
|
_turn_right_for_ms = turn_for_ms;
|
|
wait_ms(_turn_right_for_ms);
|
|
// stop drives again
|
|
stop_driving();
|
|
}
|
|
|
|
/** @brief Enables to stop the racer if during a distance measurement the measured distance is smaller then the internally stored minimal distance.
|
|
*
|
|
* This allow all of the none-single shot ultra sonic measurement methods to stopp the racer in the case the measured distance is smaller than minimal distance. This is just the enablement - you have to call one of the none-single-shot ultra sonic measurement methods continously while driving and maybe sweeping the servo.
|
|
* If the racer was stopped can be checked with stopped_at_min_distance() - it will return true in that case.
|
|
* The minimal distance can be set by the ultrasonic sensor setter methods.
|
|
* There is an example coming with the coderacer libary that you can load and get an idea how that works.
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::start_stop_at_min_distance() {
|
|
start_stop_at_min_distance(_usonic_stop_distance_cm);
|
|
}
|
|
|
|
/** @brief Enables to stopp the racer if during a distance measurement the measured distance is smaller then the specified minimal distance.
|
|
*
|
|
* This is almos the same as described for start_stop_at_min_distance(). You can specify the distance in cm here - this value will be stored internally and used by start_stop_at_min_distance() next time.
|
|
* @param min_distance_cm the minimal distance in cm
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::start_stop_at_min_distance(unsigned long min_distance_cm) {
|
|
if (false == _coderacer_stop_at_distance_enabled) {
|
|
_coderacer_stopped_at_min_distance = false;
|
|
usonic_set_stop_distance_cm(min_distance_cm);
|
|
_coderacer_stop_at_distance_enabled = true;
|
|
}
|
|
}
|
|
|
|
/** @brief Disables to stopp the racer if during a distance measurement the measured distance is smaller then the specified minimal distance.
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::stop_stop_at_min_distance() {
|
|
_coderacer_stop_at_distance_enabled = false;
|
|
}
|
|
|
|
/** @brief This will return if the codracer is in active mode or not.
|
|
*
|
|
* There is a button used to toogle between active and inactive each time it is pressed This may help you to start driving and scanning the distance by pressing the button - and as well to stop the racer by pressing the button.
|
|
* This method will set the LEDs depending on the mode and sets the servo back to center and stopps the racer when leaving the active mode. You can leave or enter the active mode as well by setting using set_active() and set_inactive().
|
|
* The button itself triggers and internall interrupt event and sets the active state - but you have to continously call that method to switch between inactive and active mode depending on the active state.
|
|
* If in inactive mode and fun is enabeld by just setting the coderacer_fun_enabled = true ... some fun will happen :-)
|
|
*
|
|
* @return True if the coderacer is in active mode. False if in inactive mode.
|
|
*/
|
|
bool CodeRacerMKII::start_stop() {
|
|
if (_coderracer_activ != coderracer_activ) {
|
|
_coderracer_activ = coderracer_activ;
|
|
if (true == _coderracer_activ) {
|
|
//set_leds_all_off();
|
|
}
|
|
else {
|
|
stop_driving();
|
|
//set_leds_all_off();
|
|
servo_set_to_center();
|
|
_servo_look_around_at_ms = millis() + random(20000, 120000);
|
|
}
|
|
}
|
|
|
|
if ((false == _coderracer_activ) && (true == coderacer_fun_enabled)) {
|
|
kitt();
|
|
look_around();
|
|
}
|
|
|
|
return(_coderracer_activ);
|
|
}
|
|
|
|
/** @brief Returns the current message saved in debug_message. Is called every 750ms in an AJAX event on the webserver.
|
|
* @return debug_message
|
|
*/
|
|
String CodeRacerMKII::wifi_printf(){
|
|
return debug_message;
|
|
}
|
|
/** @brief Almost the same as wifi_printf(), but with a specified message to send to the webserver. Can be used to send custom messages for debugging.
|
|
* @param Content the message specified by user
|
|
* @return debug_message
|
|
*/
|
|
String CodeRacerMKII::wifi_printf(String Content){
|
|
debug_message= Content;
|
|
return debug_message;
|
|
}
|
|
/** @} */ // end of group higherlevelmeths
|
|
/** @defgroup higherlevelgetters Getters and setters
|
|
* @{
|
|
*/
|
|
|
|
/** @brief Returns true if the racer was stopped at minimum distance. This set to false each time start_stop_at_min_distance() is called.
|
|
* @return True if stopped.
|
|
*/
|
|
bool CodeRacerMKII::stopped_at_min_distance() {
|
|
return(_coderacer_stopped_at_min_distance);
|
|
}
|
|
|
|
/** @brief Return true if the racer is driving - forward or backward
|
|
* @return True if driving forward or backward
|
|
*/
|
|
bool CodeRacerMKII::is_driving() {
|
|
return(_drive);
|
|
}
|
|
|
|
/** @brief Returns the duration of time that is internally stored and used for turning the racer left
|
|
* @return Time to turn left in ms
|
|
*/
|
|
unsigned long CodeRacerMKII::turn_left_for_ms() {
|
|
return(_turn_left_for_ms);
|
|
}
|
|
|
|
/** @brief Returns the duration of time that is internally stored and used for turning the racer left
|
|
* @return Time to turn left in ms
|
|
*/
|
|
unsigned long CodeRacerMKII::turn_right_for_ms() {
|
|
return(_turn_right_for_ms);
|
|
}
|
|
|
|
/** @brief Sets the coderracer_active state to inactive. If start_stop() is used - the inactive mode will be entered.
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::set_inactive() {
|
|
coderracer_activ = false;
|
|
}
|
|
|
|
/** @brief Sets the coderracer_active state to active. If start_stop() is used - the active mode will be entered.
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::set_active() {
|
|
coderracer_activ = true;
|
|
}
|
|
|
|
/** @brief Checks if coderracer_activ is set.
|
|
*
|
|
* coderracer_activ is toggled each time the left button is pressed. After power on coderracer_activ is set to False.
|
|
* @return True if coderracer_activ is set - False if not.
|
|
*/
|
|
bool CodeRacerMKII::is_active() {
|
|
return(coderracer_activ);
|
|
}
|
|
/** @brief returns the right button's current state
|
|
* _right_button_state is toggled each time the right button is pressed. It is set false by default.
|
|
* @return true if right_button_pressed is set- False if not.
|
|
*/
|
|
bool CodeRacerMKII::right_button_state() {
|
|
return(right_button_pressed);
|
|
}
|
|
/** @brief returns the variable set if an obstacle is identified by the left infrared sensor
|
|
* @return true if an obstacle is seen by the sensor, false if not
|
|
*/
|
|
bool CodeRacerMKII::is_obstacle_on_the_left(){
|
|
return(obstacle_left_side);
|
|
}
|
|
/** @brief returns the variable set if an obstacle is identified by the right infrared sensor
|
|
* @return true if an obstacle is seen by the sensor, false if not
|
|
*/
|
|
bool CodeRacerMKII::is_obstacle_on_the_right(){
|
|
return(obstacle_right_side);
|
|
}
|
|
/** @brief returns current value of the variable right_button_count
|
|
* right_button_count is set 0 by default and increments every time the right button is pressed
|
|
* @return right_button_count (an integer starting at 0)
|
|
*/
|
|
unsigned int CodeRacerMKII::button_count() {
|
|
return(right_button_count);
|
|
}
|
|
/** @brief returns the current distance driven by the Coderacer in mm
|
|
* distance is calculated with the step counter of the left wheel and a constant defined in the header file
|
|
* @return driven_distance
|
|
*/
|
|
unsigned int CodeRacerMKII::show_distance_mm(){
|
|
driven_distance= ((left_step_counter*DISTANCE_PER_TICK_MM)+ (right_step_counter*DISTANCE_PER_TICK_MM))/2;
|
|
return driven_distance;
|
|
}
|
|
/** @brief returns the current stepcount of the PE barrier of the left wheel
|
|
* @return left_step_counter
|
|
*/
|
|
unsigned int CodeRacerMKII::show_left_stepcounter(){
|
|
return left_step_counter;
|
|
}
|
|
/** @brief returns the current stepcount of the PE barrier of the right wheel
|
|
* @return right_step_counter
|
|
*/
|
|
unsigned int CodeRacerMKII::show_right_stepcounter(){
|
|
return right_step_counter;
|
|
}
|
|
|
|
/* Experimental stuff. Routines to calculate the CodeRacer's current veliocity- measured in ticks/s... but the parameters timeafterleft, timebeforeleft etc. use millis()-
|
|
so the code crashes if this is done during the interrupt rountines.
|
|
|
|
unsigned long CodeRacerMKII::show_left_time_of_last_tick(){
|
|
return timeafterleft;
|
|
}
|
|
|
|
unsigned long CodeRacerMKII::show_right_time_of_last_tick(){
|
|
return timeafterright;
|
|
}
|
|
|
|
unsigned long CodeRacerMKII::show_left_start_time(){
|
|
return timebeforeleft;
|
|
}
|
|
|
|
unsigned long CodeRacerMKII::show_right_start_time(){
|
|
return timebeforeright;
|
|
}
|
|
|
|
void CodeRacerMKII::set_left_start_time(){
|
|
timebeforeleft=0;
|
|
}
|
|
|
|
void CodeRacerMKII::set_right_start_time(){
|
|
timebeforeright=0;
|
|
}
|
|
*/
|
|
|
|
/** @} */ // end of group higherlevelgetters
|
|
/** @} */ // end of group higherlevel
|
|
|
|
//**************************************
|
|
//*** Just for fun ***
|
|
//**************************************
|
|
/** @defgroup lowerlevelfun Lower level fun stuff methods
|
|
* @{
|
|
*/
|
|
/** @brief Fun stuff - will move the servo around after a random amount of time
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::look_around()
|
|
{
|
|
if (_servo_look_around_at_ms < millis()) {
|
|
_servo_look_around_at_ms = millis() + random(FUN_MIN_PAUSE_MS, FUN_MAX_PAUSE_MS);
|
|
servo_set_to_left();
|
|
delay(500);
|
|
servo_set_to_right();
|
|
delay(800);
|
|
servo_set_to_center();
|
|
delay(300);
|
|
servo_set_to_left();
|
|
delay(100);
|
|
servo_set_to_center();
|
|
}
|
|
}
|
|
|
|
/** @brief Fun stuff - you know Knight Rider...
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::kitt()
|
|
{
|
|
if (millis() - _last_led_switched_at_ms > LED_SWITCH_MS) {
|
|
_last_led_switched_at_ms = millis();
|
|
if (_last_led_on >= 6) {
|
|
_led_count = -1;
|
|
}
|
|
else if (_last_led_on <= 0) {
|
|
_led_count = 1;
|
|
}
|
|
_last_led_on = _last_led_on + _led_count;
|
|
switch (_last_led_on) {
|
|
case 0:
|
|
set_leds(LEDOFF, LEDOFF, LEDOFF, LEDOFF, LEDOFF, LEDOFF);
|
|
break;
|
|
case 1:
|
|
set_leds(LEDON, LEDOFF, LEDOFF, LEDOFF, LEDOFF, LEDOFF);
|
|
break;
|
|
case 2:
|
|
set_leds(LEDOFF, LEDON, LEDOFF, LEDOFF, LEDOFF, LEDOFF);
|
|
break;
|
|
case 3:
|
|
set_leds(LEDOFF, LEDOFF, LEDON, LEDOFF, LEDOFF, LEDOFF);
|
|
break;
|
|
case 4:
|
|
set_leds(LEDOFF, LEDOFF, LEDOFF, LEDON, LEDOFF, LEDOFF);
|
|
break;
|
|
case 5:
|
|
set_leds(LEDOFF, LEDOFF, LEDOFF, LEDOFF, LEDON, LEDOFF);
|
|
break;
|
|
case 6:
|
|
set_leds(LEDOFF, LEDOFF, LEDOFF, LEDOFF, LEDOFF, LEDON);
|
|
}
|
|
}
|
|
}
|
|
|
|
/** @brief Reads the four little switches and returns a value from 1-4, depending which of them is turned on.
|
|
* Only the first switch turned on is read.
|
|
* @return an integer from 1 to 4, representing the number of the switch currently active
|
|
*/
|
|
int CodeRacerMKII::switch_check()
|
|
{
|
|
int generalMode=0;
|
|
if(digitalRead(_switch_1)==LOW)
|
|
{
|
|
generalMode=1;
|
|
return generalMode;
|
|
}
|
|
if(digitalRead(_switch_2)== LOW)
|
|
{
|
|
generalMode=2;
|
|
return generalMode;
|
|
}
|
|
if(digitalRead(_switch_3)==LOW)
|
|
{
|
|
generalMode=3;
|
|
return generalMode;
|
|
}
|
|
if(digitalRead(_switch_4)==LOW)
|
|
{
|
|
generalMode=4;
|
|
return generalMode;
|
|
}
|
|
return generalMode;
|
|
}
|
|
|
|
/** @} */ // end of group lowerlevelfun
|
|
|
|
|
|
//**************************************
|
|
//*** Bluetooth ***
|
|
//**************************************
|
|
/** @defgroup lowerlevelbluetooth Lower level bluetooth methods
|
|
* @{
|
|
*/
|
|
/** @brief starting the bluetooth service for the coderacer if not already started
|
|
* @param name the device will be listed by that name in your bluetooth device lists
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::bt_start(String name)
|
|
{
|
|
if (false == _bluetoothcreated) {
|
|
_BTSerial = new BluetoothSerial();
|
|
_BTSerial->begin(name);
|
|
_bluetoothcreated = true;
|
|
}
|
|
}
|
|
|
|
/** @brief enables the code to stop if there was no incoming message for the specified time. This will be checked everytime bt_getXXX or bt_msgavailable is called.
|
|
* @param timeout after that duration of milliseconds without an incoming bluetooth message the coderacer will be stopped. 0 means this stoppinf is disabled.
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::bt_enable_stopOnLostConnection(unsigned long timeout)
|
|
{
|
|
_bt_stopOnLostConnection_timeout_ms = timeout;
|
|
}
|
|
|
|
/** @brief enables the code to stop if there was no incoming message for 1 second. This will be checked everytime bt_getXXX or bt_msgavailable is called.
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::bt_enable_stopOnLostConnection()
|
|
{
|
|
_bt_stopOnLostConnection_timeout_ms = 1000;
|
|
}
|
|
|
|
/** @brief Disables the code to stop if there was no incoming message for a certain duration of time.
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::bt_disable_stopOnLostConnection()
|
|
{
|
|
_bt_stopOnLostConnection_timeout_ms = 0;
|
|
}
|
|
|
|
/** @brief gets the bluetooth message string until a delimiter of 0
|
|
* @return will return the string. If nothing is availbale or the service is not started it will return an empty string.
|
|
*/
|
|
String CodeRacerMKII::bt_getString()
|
|
{
|
|
return bt_getString(0);
|
|
}
|
|
|
|
/** @brief gets the bluetooth message string until a specified delimiter
|
|
* @return will return the string. If nothing is availbale or the service is not started it will return an empty string.
|
|
*/
|
|
String CodeRacerMKII::bt_getString(uint8_t delimiterbyte)
|
|
{
|
|
String readstring = "";
|
|
if (bt_msgavailable())
|
|
{
|
|
readstring = _BTSerial->readStringUntil(delimiterbyte);
|
|
if (find(_bt_ignoremsgs.begin(), _bt_ignoremsgs.end(), readstring) != _bt_ignoremsgs.end())
|
|
{
|
|
readstring = "";
|
|
}
|
|
}
|
|
return(readstring);
|
|
}
|
|
|
|
/** @brief add a String to a list of Strings that will be ignored if this is received via blue tooth. Ignores means - it will be read from the pipe but not returned to user code. But it will reset the message timeout counter.
|
|
* @param stringtoignore the String that has to be ignored. Will be added to the internal list if not already there.
|
|
*/
|
|
void CodeRacerMKII::bt_addStringToIgnoreList(String stringtoignore)
|
|
{
|
|
std::vector<String>::iterator it;
|
|
if (stringtoignore.length() > 0)
|
|
{
|
|
it = find(_bt_ignoremsgs.begin(), _bt_ignoremsgs.end() ,stringtoignore);
|
|
if (it == _bt_ignoremsgs.end())
|
|
{
|
|
_bt_ignoremsgs.push_back(stringtoignore);
|
|
}
|
|
}
|
|
}
|
|
|
|
/** @brief removes a String from the list of Strings that will be ignored if this is received via blue tooth. Ignores means - it will be read from the pipe but not returned to user code. But it will reset the message timeout counter.
|
|
* @param stringtoignore the String that has to be removed from the ignore list.
|
|
*/
|
|
void CodeRacerMKII::bt_removeStringFromIgnoreList(String stringtoignore)
|
|
{
|
|
std::vector<String>::iterator it;
|
|
if (stringtoignore.length() > 0)
|
|
{
|
|
it = find(_bt_ignoremsgs.begin(), _bt_ignoremsgs.end(), stringtoignore);
|
|
if (it != _bt_ignoremsgs.end())
|
|
{
|
|
_bt_ignoremsgs.erase(it);
|
|
}
|
|
}
|
|
}
|
|
|
|
/** @brief Clears the list of Strings that will be ignored if this is received via blue tooth. All elements of the list will be deleted from the list.
|
|
*/
|
|
void CodeRacerMKII::bt_clearIgnoreList()
|
|
{
|
|
_bt_ignoremsgs.clear();
|
|
}
|
|
|
|
|
|
/** @brief checks if a bluetooth is available. Will also stop the coderacer if there was nor message received for a certain time - and if stopping was enabled .
|
|
* @return true if a message is available , false if not message is available or the service was not started
|
|
*/
|
|
bool CodeRacerMKII::bt_msgavailable()
|
|
{
|
|
bool rc = false;
|
|
if (true == _bluetoothcreated) {
|
|
if(_BTSerial->available())
|
|
{
|
|
rc = true;
|
|
_bt_lastmessagereceived = millis();
|
|
}
|
|
if (_bt_stopOnLostConnection_timeout_ms > 0)
|
|
{
|
|
if ((millis() - _bt_lastmessagereceived) > _bt_stopOnLostConnection_timeout_ms)
|
|
{
|
|
stop_driving();
|
|
}
|
|
}
|
|
|
|
}
|
|
return rc;
|
|
}
|
|
|
|
/** @} */ // end of group lowerlevelbluetooth
|
|
|
|
//**************************************
|
|
//*** Servo drive lower level control ***
|
|
//**************************************
|
|
/** @defgroup lowerlevelservo Lower level servo drive methods and getters
|
|
* @{
|
|
*/
|
|
/** @defgroup lowerlevelservomeths Methods
|
|
* @{
|
|
*/
|
|
/** @brief Overwrites the default settings taken from header file by the parameters given to this method
|
|
* @param pos_center The postion at which the servo moves to straight forward. Default is 90. Allowed 10 <= pos_center <= 170.
|
|
* @param pos_left The postion at which the servo moves to the left. Default is 170. Allowed 10 <= pos_center <= 170.
|
|
* @param pos_right The postion at which the servo moves to the right. Default is 10. Allowed 10 <= pos_center <= 170.
|
|
* @param sweep_left_pos If the servo is sweeping from left to the right - this defines the most left postion. Default is 140. Allowed 10 <= pos_center <= 170.
|
|
* @param sweep_right_pos If the servo is sweeping from left to the right - this defines the most right postion. Default is 40. Allowed 10 <= pos_center <= 170.
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::servo_settings(uint8_t pos_center, uint8_t pos_left, uint8_t pos_right, uint8_t sweep_left_pos, uint8_t sweep_right_pos)
|
|
{
|
|
servo_center_pos = pos_center;
|
|
servo_left_pos = pos_left;
|
|
servo_right_pos = pos_right;
|
|
servo_sweep_left_pos = sweep_left_pos;
|
|
servo_sweep_right_pos = sweep_right_pos;
|
|
}
|
|
|
|
/** @brief Turns sweeping of the servo from left to right and back on.
|
|
*
|
|
* The sweeping range is defind by #servo_sweep_left_pos and #servo_sweep_right_pos attributes. Both can be set by either servo_settings() or as public members.
|
|
* Every time servo_sweep() is called the servo is driven by 5 steps until either #servo_sweep_left_pos or #servo_sweep_right_pos is reached. Then it will turn the
|
|
* direction and step to the other side every time this method is called.
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::servo_sweep()
|
|
{
|
|
uint8_t position;
|
|
_servo_sweep = true;
|
|
if (millis() - _servo_position_set_at_ms > SERVO_SWEEP_MS) {
|
|
position = _servo_position + _servo_sweep_step;
|
|
//sprintf(_debugmsg,"[%s] current position=%ld newpostion=%ld", __func__, _servo_position, position);
|
|
if ((position >= servo_sweep_left_pos) || (position >= SERVO_MAX_POSITION)) {
|
|
position = servo_sweep_left_pos;
|
|
_servo_sweep_step = _servo_sweep_step_right;
|
|
}
|
|
if ((position <= servo_sweep_right_pos) || (position <= SERVO_MIN_POSITION)) {
|
|
position = servo_sweep_right_pos;
|
|
_servo_sweep_step = _servo_sweep_step_left;
|
|
}
|
|
_servo_set_position(position);
|
|
}
|
|
}
|
|
|
|
/** @brief Drives the servo to the postion that is defined by #servo_right_pos
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::servo_set_to_right()
|
|
{
|
|
servo_set_position_wait(servo_right_pos);
|
|
}
|
|
|
|
/** @brief Drives the servo to the postion that is defined by #servo_left_pos
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::servo_set_to_left()
|
|
{
|
|
servo_set_position_wait(servo_left_pos);
|
|
}
|
|
|
|
/** @brief Drives the servo to the postion that is defined by #servo_center_pos
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::servo_set_to_center()
|
|
{
|
|
servo_set_position_wait(servo_center_pos);
|
|
}
|
|
|
|
/** @brief Drive the servo to the postion given to this method
|
|
*
|
|
* The method will wait until the servo has reached its new position.
|
|
* @param position Position the servo will be drived to. Allowed are values 10<=postion<=170. 10 is at the right hand side, 170 at the left hand side.
|
|
* @return The new servo position
|
|
*/
|
|
uint8_t CodeRacerMKII::servo_set_position_wait(uint8_t position)
|
|
{
|
|
_servo_sweep = false;
|
|
unsigned long wait_time_ms = _servo_set_position(position);
|
|
wait_ms(wait_time_ms);
|
|
return(_servo_position);
|
|
}
|
|
|
|
/** @brief Drive the servo to the postion given to this method
|
|
*
|
|
* The method will not wait until the servo has reached its new position.
|
|
* @param position Position the servo will be drived to. Allowed are values 10<=postion<=170. 10 is at the right hand side, 170 at the left hand side.
|
|
* @return The time in ms the servo will need to reach the new position
|
|
*/
|
|
unsigned long CodeRacerMKII::servo_set_position(uint8_t position)
|
|
{
|
|
_servo_sweep = false;
|
|
unsigned long wait_time_ms = _servo_set_position(position);
|
|
return(wait_time_ms);
|
|
}
|
|
|
|
unsigned long CodeRacerMKII::_servo_set_position(uint8_t position)
|
|
{
|
|
uint8_t _position = position;
|
|
uint8_t absdiff;
|
|
|
|
if (position < SERVO_MIN_POSITION) {
|
|
_position = SERVO_MIN_POSITION;
|
|
}
|
|
else if (position > SERVO_MAX_POSITION) {
|
|
_position = SERVO_MAX_POSITION;
|
|
}
|
|
_servo->write(_position);
|
|
// wait minimal delay to avoid code collaps
|
|
wait_ms(SERVO_SET_1TICK_POSITION_DELAY_MS);
|
|
absdiff = abs(_servo_position - _position);
|
|
if (absdiff > 1) {
|
|
_servo_position_eta_in_ms = absdiff * SERVO_SET_1TICK_POSITION_DELAY_MS;
|
|
}
|
|
else {
|
|
_servo_position_eta_in_ms = 0;
|
|
}
|
|
|
|
_servo_position_set_at_ms = millis();
|
|
_servo_position = _position;
|
|
|
|
return(_servo_position_eta_in_ms);
|
|
}
|
|
|
|
/** @} */ // end of group lowerlevelservomeths
|
|
/** @defgroup lowerlevelservogetters Getters
|
|
* @{
|
|
*/
|
|
|
|
/** @brief Get the actual position of the servo
|
|
* @return Actual position of the servo
|
|
*/
|
|
uint8_t CodeRacerMKII::servo_position() {
|
|
return(_servo_position);
|
|
}
|
|
|
|
/** @brief Get the system time in ms the servo was set to the actual position
|
|
* @return System time in ms the servo was set
|
|
*/
|
|
unsigned long CodeRacerMKII::servo_position_set_at_ms() {
|
|
return(_servo_position_set_at_ms);
|
|
}
|
|
|
|
/** @brief Get the system time in ms the servo will reach its position
|
|
* This is an estimated time.
|
|
* If this is a time in the future, the servo may still moving.
|
|
* If this is a time in the past , the servo should have reached its postion already.
|
|
* @return System time in ms the servo will reach its position
|
|
*/
|
|
unsigned long CodeRacerMKII::servo_position_eta_in_ms() {
|
|
return(_servo_position_eta_in_ms);
|
|
}
|
|
|
|
/** @} */ // end of group lowerlevelservogetters
|
|
/** @} */ // end of group lowerlevelservo
|
|
|
|
|
|
//**************************************
|
|
//*** Ultrasonic lower level control ***
|
|
//**************************************
|
|
/** @defgroup lowerlevelus Lower level ultra sonic methods and getters
|
|
* @{
|
|
*/
|
|
/** @defgroup lowerlevelusmeths Methods
|
|
* @{
|
|
*/
|
|
|
|
/** @brief Measures the distance to the next object in front of the ultra sonic sensor in cm.
|
|
*
|
|
* This is the medium one out of 3 measurements. The maximum measured distance is about 100cm and defined by the US_MAX_ECHO_TIME_US setting in the header file.
|
|
* @return The measured distance in cm.
|
|
*/
|
|
unsigned long CodeRacerMKII::usonic_measure_cm()
|
|
{
|
|
return(usonic_measure_cm(US_MAX_ECHO_TIME_US));
|
|
}
|
|
|
|
/** @brief Measures the distance to the next object in front of the ultra sonic sensor in microseconds.
|
|
*
|
|
* This is the medium one out of 3 measurements. The maximum measured distance is about 6000 microseconds and defined by the US_MAX_ECHO_TIME_US setting in the header file.
|
|
* @return The measured distance in microseconds.
|
|
*/
|
|
unsigned long CodeRacerMKII::usonic_measure_us()
|
|
{
|
|
return(usonic_measure_us(US_MAX_ECHO_TIME_US));
|
|
}
|
|
|
|
/** @brief Measures the distance to the next object in front of the ultra sonic sensor in cm.
|
|
*
|
|
* This is the medium one out of 3 measurements. Be careful with high values for max_echo_run_time_us - this may increase run time due to the fact that if there is nothing in range of the sensor it will wait until this specified run time of the echo is over.
|
|
* The maximum range the sensor is specified for is about 300cm.
|
|
* @param max_echo_run_time_us Defines the maximum echo run time and by that the maximum of distance that can be measured.
|
|
* @return The measured distance in cm.
|
|
*/
|
|
unsigned long CodeRacerMKII::usonic_measure_cm(unsigned long max_echo_run_time_us)
|
|
{
|
|
unsigned long echo_runtime_us = usonic_measure_us(max_echo_run_time_us);
|
|
unsigned long distance_cm = echo_runtime_us * 0.0172;
|
|
//Serial.print("MEASURE_DISTANCE. Distance in cm is: ");
|
|
//Serial.println(distance_cm);
|
|
_usonic_distance_cm = distance_cm;
|
|
return(distance_cm);
|
|
}
|
|
|
|
/** @brief Measures the distance to the next object in front of the ultra sonic sensor in microseconds.
|
|
*
|
|
* This is the medium one out of 3 measurements. Be careful with high values for max_echo_run_time_us - this may increase run time due to the fact that if there is nothing in range of the sensor it will wait until this specified run time of the echo is over.
|
|
* The maximum range the sensor is specified for is about 300cm.
|
|
* @param max_echo_run_time_us Defines the maximum echo run time in microseconds and by that the maximum of distance that can be measured.
|
|
* @return The measured distance in microseconds.
|
|
*/
|
|
unsigned long CodeRacerMKII::usonic_measure_us(unsigned long max_echo_run_time_us)
|
|
{
|
|
unsigned long echo_runtime_us[3] = { 0,0,0 };
|
|
uint8_t measnr = 0;
|
|
|
|
do {
|
|
echo_runtime_us[measnr] = usonic_measure_single_shot_us(max_echo_run_time_us);
|
|
if (echo_runtime_us[measnr] > 200) {
|
|
measnr++;
|
|
}
|
|
} while (measnr < 3);
|
|
|
|
// we will take the medium out of 3 values ...
|
|
if (echo_runtime_us[0] > echo_runtime_us[1]) { std::swap(echo_runtime_us[0], echo_runtime_us[1]); }
|
|
if (echo_runtime_us[1] > echo_runtime_us[2]) { std::swap(echo_runtime_us[1], echo_runtime_us[2]); }
|
|
if (echo_runtime_us[0] > echo_runtime_us[1]) { std::swap(echo_runtime_us[0], echo_runtime_us[1]); }
|
|
|
|
//Serial.print("MEASURE_DISTANCE_US. Echo runtime in us is: ");
|
|
//Serial.println(echo_runtime_us[1]);
|
|
|
|
// if the stop at minimal distance is enabeled - check for minimal distance is reached
|
|
if (true == _coderacer_stop_at_distance_enabled) {
|
|
if (echo_runtime_us[1] <= _usonic_stop_distance_us) {
|
|
_coderacer_stopped_at_min_distance = true;
|
|
stop_driving();
|
|
_coderacer_stop_at_distance_enabled = false;
|
|
}
|
|
}
|
|
_usonic_distance_us = echo_runtime_us[1];
|
|
return(echo_runtime_us[1]);
|
|
}
|
|
|
|
/** @brief Measures the distance to the next object in front of the ultra sonic sensor in cm.
|
|
*
|
|
* This is a one shot measurement. The maximum measured distance is about 6000 microseconds and defined by the US_MAX_ECHO_TIME_US setting in the header file.
|
|
* @return The measured distance in cm.
|
|
*/
|
|
unsigned long CodeRacerMKII::usonic_measure_single_shot_cm()
|
|
{
|
|
return(usonic_measure_single_shot_cm(US_MAX_ECHO_TIME_US));
|
|
}
|
|
|
|
/** @brief Measures the distance to the next object in front of the ultra sonic sensor in microseconds.
|
|
*
|
|
* This is a one shot measurement. The maximum measured distance is about 6000 microseconds and defined by the US_MAX_ECHO_TIME_US setting in the header file.
|
|
* @return The measured distance in microseconds.
|
|
*/
|
|
unsigned long CodeRacerMKII::usonic_measure_single_shot_us()
|
|
{
|
|
return(usonic_measure_single_shot_us(US_MAX_ECHO_TIME_US));
|
|
}
|
|
|
|
/** @brief Measures the distance to the next object in front of the ultra sonic sensor in cm.
|
|
*
|
|
* This is a one shot measurement. Be careful with high values for max_echo_run_time_us - this may increase run time due to the fact that if there is nothing in range of the sensor it will wait until this specified run time of the echo is over.
|
|
* The maximum range the sensor is specified for is about 300cm.
|
|
* @param max_echo_run_time_us Defines the maximum echo run time in microseconds and by that the maximum of distance that can be measured.
|
|
* @return The measured distance in cm.
|
|
*/
|
|
unsigned long CodeRacerMKII::usonic_measure_single_shot_cm(unsigned long max_echo_run_time_us)
|
|
{
|
|
// convert into cm ... 344m/sec is the speed of noise - thus 34400cm/sec ... or 34,400cm/milisec ... or 0,0344cm/microsec
|
|
// the echo has to go the distance twice - forth and back - so the duration has to be the half of the measured one
|
|
// distance_cm = echo_duration/2 * 0,0344 or distance_cm = echo_duration/2 / 29,1 or distance_cm = echo_duration * 0,0172
|
|
// distance_cm = (echo_duration/2) / 29.1;
|
|
unsigned long echo_runtime_us = usonic_measure_single_shot_us(max_echo_run_time_us);
|
|
unsigned long distance_cm = echo_runtime_us * 0.0172;
|
|
//Serial.print("MEASURE_DISTANCE. Distance in cm is: ");
|
|
//Serial.println(distance_cm);
|
|
_usonic_distance_cm = distance_cm;
|
|
return(distance_cm);
|
|
}
|
|
|
|
/** @brief Measures the distance to the next object in front of the ultra sonic sensor in microseconds.
|
|
*
|
|
* This is a one shot measurement. Be careful with high values for max_echo_run_time_us - this may increase run time due to the fact that if there is nothing in range of the sensor it will wait until this specified run time of the echo is over.
|
|
* The maximum range the sensor is specified for is about 300cm.
|
|
* @param max_echo_run_time_us Defines the maximum echo run time in microseconds and by that the maximum of distance that can be measured.
|
|
* @return The measured distance in microseconds.
|
|
*/
|
|
unsigned long CodeRacerMKII::usonic_measure_single_shot_us(unsigned long max_echo_run_time_us)
|
|
{
|
|
unsigned long echo_runtime_us;
|
|
// start measurement - send a short pulse "HIGH" to the TRIG pin of the ultrasonic sensor
|
|
pinMode(_us_echo_pin, OUTPUT);
|
|
pinMode(_us_echo_pin, INPUT);
|
|
digitalWrite(_us_trigger_pin, LOW);
|
|
delayMicroseconds(2);
|
|
digitalWrite(_us_trigger_pin, HIGH);
|
|
delayMicroseconds(10);
|
|
digitalWrite(_us_trigger_pin, LOW);
|
|
// measure runtime in microseconds until the ECHO pin aof the sensor goes HIGH
|
|
echo_runtime_us = pulseInLong(_us_echo_pin, HIGH, max_echo_run_time_us);
|
|
if (echo_runtime_us == 0) {
|
|
echo_runtime_us = max_echo_run_time_us; // US_MAX_ECHO_TIME_US;
|
|
}
|
|
//Serial.print("MEASURE_DISTANCE_US. Echo runtime in us is: ");
|
|
//Serial.println(echo_runtime_us);
|
|
_usonic_distance_us = echo_runtime_us;
|
|
return(echo_runtime_us);
|
|
}
|
|
|
|
/** @} */ // end of group lowerlevelusmethods
|
|
/** @defgroup lowerlevelusgetters Setters and getters
|
|
* @{
|
|
*/
|
|
|
|
/** @brief Sets the stop distance in cm.
|
|
*
|
|
* If start_stop_at_min_distance() is used and distance measured with one of the measurement methods - the racer will be stopped immediately. All except the singe shot methods of the ultra sonic measurements methods supports that.
|
|
* Internally the stop distance will be set as both - in cm and in microseconds.
|
|
* @param stop_distance_cm Distance in cm the racer will be stopped if that features was enabled by start_stop_at_min_distance() before.
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::usonic_set_stop_distance_cm(unsigned long stop_distance_cm)
|
|
{
|
|
_usonic_stop_distance_us = stop_distance_cm * 58.14;
|
|
}
|
|
|
|
/** @brief Sets the stop distance in cm.
|
|
*
|
|
* If start_stop_at_min_distance() is used and distance measured with one of the measurement methods - the racer will be stopped immediately. All except the singe shot methods of the ultra sonic measurements methods supports that.
|
|
* Internally the stop distance will be set as both - in cm and in microseconds.
|
|
* @param stop_distance_us Distance in cm the racer will be stopped if that features was enabled by start_stop_at_min_distance() before.
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::usonic_set_stop_distance_us(unsigned long stop_distance_us)
|
|
{
|
|
_usonic_stop_distance_us = stop_distance_us;
|
|
}
|
|
|
|
/** @brief Returns the last measured distance in microseconds
|
|
* @return Distance in microseconds
|
|
*/
|
|
unsigned long CodeRacerMKII::usonic_distance_us() {
|
|
return(_usonic_distance_us);
|
|
}
|
|
|
|
/** @brief Returns the last measured distance in cm
|
|
* @return Distance in cm
|
|
*/
|
|
unsigned long CodeRacerMKII::usonic_distance_cm() {
|
|
return(_usonic_distance_cm);
|
|
}
|
|
|
|
/** @} */ // end of group lowerlevelusgetters
|
|
/** @} */ // end of group lowerlevelus
|
|
|
|
|
|
//**************************************
|
|
//*** Drives lower level control ***
|
|
//**************************************
|
|
|
|
/** @defgroup lowerleveldrives Lower level drives methods and getters
|
|
* @{
|
|
*/
|
|
/** @defgroup lowerleveldrivesmeths Methods
|
|
* @{
|
|
*/
|
|
|
|
/** @brief Overwrites the values for the speed of the race. This will replace the defaults set by the values in the header file.
|
|
* @param drive_left_speed Speed of the left side drive
|
|
* @param drive_right_speed Speed of the right side drive
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::speed_settings(uint8_t drive_left_speed, uint8_t drive_right_speed)
|
|
{
|
|
_drive_left_speed = drive_left_speed;
|
|
_drive_right_speed = drive_right_speed;
|
|
}
|
|
/** @brief Overwrites some drive settings. This will replace the defaults set by the values in the header file.
|
|
* @param drive_left_speed Speed of the left side drive
|
|
* @param drive_right_speed Speed of the right side drive
|
|
* @param turn_left_for_ms Time in ms the racer will turn to the left around its center if turn_left() is called
|
|
* @param turn_right_for_ms Time in ms the racer will turn to the right around its center if turn_right() is called
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::drives_settings(uint8_t drive_left_speed, uint8_t drive_right_speed, unsigned long turn_left_for_ms, unsigned long turn_right_for_ms)
|
|
{
|
|
_drive_left_speed = drive_left_speed;
|
|
_drive_right_speed = drive_right_speed;
|
|
_turn_left_for_ms = turn_left_for_ms;
|
|
_turn_right_for_ms = turn_right_for_ms;
|
|
}
|
|
/** @brief "Resets" the Racer. In detail, stops driving as well as setting both step counters to 0. The variable for the distance driven also gets set to 0,
|
|
* speed settings are reverted to their default (128), if a change to them has been made. Finally, the servo gets set to the center.
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::Reset_Stats(){
|
|
stop_driving();
|
|
left_step_counter=0;
|
|
right_step_counter=0;
|
|
driven_distance=0;
|
|
speed_settings(128, 128);
|
|
servo_set_to_center();
|
|
wifi_printf("Resetted stats.");
|
|
}
|
|
|
|
void CodeRacerMKII::set_drives_stop_left_right() {
|
|
set_drive_left_state(DRIVESTOP);
|
|
set_drive_right_state(DRIVESTOP);
|
|
}
|
|
|
|
/** @brief Sets both of the drives to a specified drivestate (DRIVESTOP, DRIVEFRWD, DRIVEBACK)
|
|
* @param stateleft drivestate to set for the left side drive
|
|
* @param stateright drivestate to set for the right side drive
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::set_drives_states_left_right(drivestate stateleft, drivestate stateright) {
|
|
set_drive_left_state(stateleft);
|
|
set_drive_right_state(stateright);
|
|
}
|
|
|
|
/** @brief Sets the left side drive to the specified drivestate (DRIVESTOP, DRIVEFRWD, DRIVEBACK)
|
|
* @param state drivestate to set for the left side drive
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::set_drive_left_state(drivestate state) {
|
|
set_drive_state(state, _drive_left_frwd_bkwrd_pin, _drive_left_enable_pin);
|
|
}
|
|
|
|
/** @brief Sets the right side drive to the specified drivestate (DRIVESTOP, DRIVEFRWD, DRIVEBACK)
|
|
* @param state drivestate to set for the right side drive
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::set_drive_right_state(drivestate state) {
|
|
set_drive_state(state, _drive_right_frwd_bkwrd_pin, _drive_right_enable_pin);
|
|
}
|
|
|
|
/** @brief Sets the specified drivestate for the drive connected to the sepecified pins (DRIVESTOP, DRIVEFRWD, DRIVEBACK)
|
|
* @param state drivestate to set for the connected drive
|
|
* @param frwdpin Pin the forward signal of the drive device driver is connected at
|
|
* @param backpin Pin the backward signal of the drive device driver is connected at
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::set_drive_state(drivestate state, uint8_t frwdbackpin, uint8_t enable_pin ) {
|
|
switch (state) {
|
|
case DRIVESTOP:
|
|
set_drive_speed(0, enable_pin);
|
|
break;
|
|
case DRIVEFRWD:
|
|
digitalWrite(frwdbackpin, HIGH);
|
|
break;
|
|
case DRIVEBACK:
|
|
digitalWrite(frwdbackpin, LOW);
|
|
break;
|
|
}
|
|
}
|
|
/** @brief enables or disables a syncronized halting if the method stop_driving() is used. if disabled, the left engine will continue running.
|
|
* @param state a boolean expression to enable or disable syncstop. this variable is set false by default.
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::set_syncstop(bool state){
|
|
if (1== state)
|
|
{
|
|
syncstop=1;
|
|
}
|
|
else syncstop=0;
|
|
}
|
|
/** @brief enables or disables obstacle_stop and by that, enables or disables halting if an obstacle is identified by either of the infrared sensors
|
|
* @param state a boolean expression to enable or disable obstacle_stop. this variable is set false by default.
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::set_obstacle_stop(bool state){
|
|
if (1== state)
|
|
{
|
|
obstacle_stop= 1;
|
|
}
|
|
else obstacle_stop=0;
|
|
}
|
|
|
|
/** @brief Sets the speed for both of the drives.
|
|
*
|
|
* The drive will run with that speed afterwards. The direction of the drive has to be specfied with one of the set_drive_state methods before
|
|
* @param speedleft speed of the left side drive. 0<=speed<=255
|
|
* @param speedright speed of the right side drive. 0<=speed<=255
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::set_drives_speed_left_right(uint8_t speedleft, uint8_t speedright) {
|
|
set_drive_left_speed(speedleft);
|
|
set_drive_right_speed(speedright);
|
|
}
|
|
|
|
/** @brief Sets the speed for the left side drive.
|
|
*
|
|
* The drive will run with that speed afterwards. The direction of the drive has to be specfied with one of the set_drive_state methods before
|
|
* @param speed speed of the left side drive. 0<=speed<=255
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::set_drive_left_speed(uint8_t speed) {
|
|
uint16_t max_speed = speed / _calibration_factor;
|
|
if (speed > max_speed)
|
|
set_drive_speed(max_speed, _drive_left_enable_pin);
|
|
else
|
|
set_drive_speed(speed, _drive_left_enable_pin);
|
|
}
|
|
|
|
|
|
|
|
/** @brief Sets the speed for the right side drive.
|
|
*
|
|
* The drive will run with that speed afterwards. The direction of the drive has to be specfied with one of the set_drive_state methods before
|
|
* @param speed speed of the right side drive. 0<=speed<=255
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::set_drive_right_speed(uint8_t speed) {
|
|
uint max_speed = speed * _calibration_factor;
|
|
if (speed > max_speed)
|
|
set_drive_speed(max_speed, _drive_right_enable_pin);
|
|
else
|
|
set_drive_speed(speed, _drive_right_enable_pin);
|
|
}
|
|
|
|
/** @brief Sets the speed for the drive of the enable pin connected to the specified pin.
|
|
*
|
|
* The drive will run with that speed afterwards. The direction of the drive has to be specfied with one of the set_drive_state methods before
|
|
* @param speed speed of the drive. 0<=speed<=255
|
|
* @param enablepin Pin the drives device driver enable pin is connected at
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::set_drive_speed(uint8_t speed, uint8_t enablepin) {
|
|
_analog_write(enablepin, (int)speed);
|
|
}
|
|
|
|
/** @} */ // end of group lowerleveldrivesmethods
|
|
/** @defgroup lowerleveldrivesgetters Getters
|
|
* @{
|
|
*/
|
|
|
|
/** @brief Get the setting for the speed of the right side drive
|
|
* @return Speed of the right side drive
|
|
*/
|
|
uint8_t CodeRacerMKII::drive_right_speed() {
|
|
return _drive_right_speed;
|
|
}
|
|
|
|
/** @brief Get the setting for the speed of the left side drive
|
|
* @return Speed of the left side drive
|
|
*/
|
|
uint8_t CodeRacerMKII::drive_left_speed() {
|
|
return(_drive_left_speed);
|
|
}
|
|
|
|
void CodeRacerMKII::_analog_write(uint8_t pin, uint8_t speed) {
|
|
if (pin == _drive_left_enable_pin) {
|
|
ledcWrite(DRIVE_PWM_LEFT_CHANNEL, speed);
|
|
}
|
|
if (pin == _drive_right_enable_pin) {
|
|
ledcWrite(DRIVE_PWM_RIGHT_CHANNEL, speed);
|
|
}
|
|
}
|
|
/** @brief tells the coderacer to drive a specified number of wheel turns forward. requires a positive integer.
|
|
* @param turnsleft sets the number of turns for the left wheel
|
|
* @param turnsright sets the number of turns for the right wheel
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::drive_turns(unsigned int turnsleft, unsigned int turnsright){
|
|
drive_ticks(turnsleft*TURN, turnsright*TURN);
|
|
}
|
|
/** @brief tells the coderacer to drive a specified number of ticks of the PE barriers. requires a positive integer.
|
|
* @param tickleft sets the number of ticks for the left wheel
|
|
* @param tickright sets the number of ticks for the right wheel
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::drive_ticks(unsigned int tickleft, unsigned int tickright){
|
|
drive_ticks_left(tickleft, false);
|
|
drive_ticks_right(tickright, false);
|
|
drive_forward();
|
|
}
|
|
/** @brief tells the coderacer to turn by a specified number of degrees to the left or to the right. if the value is positive, turn clockwise, if negative turn counter- clockwise.
|
|
* the speed of the turn is defined by the normal driving speed.
|
|
* @param degree tells the coderacer how many degrees it has to turn.
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::turn_degree(int degree){
|
|
// sets the drive states for both wheels and by that the direction the racer will turn.
|
|
if(degree>0)
|
|
{
|
|
set_drives_states_left_right(DRIVEFRWD, DRIVEBACK);
|
|
}
|
|
else if (degree<0)
|
|
{
|
|
set_drives_states_left_right(DRIVEBACK, DRIVEFRWD);
|
|
degree= abs(degree);
|
|
}
|
|
else {set_drives_states_left_right(DRIVESTOP, DRIVESTOP);}
|
|
// calculates the number of ticks for both wheels and tells the racer to drive with the internally stored driving speed
|
|
int Ticks= degree*TICKS_OF_ONE_DEGREE;
|
|
drive_ticks_left(Ticks, false);
|
|
drive_ticks_right(Ticks, false);
|
|
set_drives_speed_left_right(_drive_left_speed, _drive_right_speed);
|
|
_drive= true;
|
|
while(true==is_driving()){}
|
|
|
|
}
|
|
/** @brief tells the coderacer to drive a specified number of ticks for the left wheel.
|
|
* @param Ticks number of ticks
|
|
* @param drive can be told to be false- if that is the case, the racer won't start driving but will only set the new stopcount for the left wheel
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::drive_ticks_left(unsigned int Ticks, bool drive = true ){
|
|
left_step_stopcount = left_step_counter+ Ticks;
|
|
if(true == drive){drive_forward();}
|
|
}
|
|
/** @brief tells the coderacer to drive a specified number of ticks for the right wheel.
|
|
* @param Ticks number of ticks
|
|
* @param drive can be told to be false- if that is the case, the racer won't start driving but will only set the new stopcount for the right wheel
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::drive_ticks_right(unsigned int Ticks, bool drive = true){
|
|
right_step_stopcount = right_step_counter+ Ticks;
|
|
if(true == drive){drive_forward();}
|
|
}
|
|
/** @brief tells the coderacer to drive a specified distance in mm. requires two values for both the left and the right wheel.
|
|
* @param leftdistance the distance to be driven by the left wheel
|
|
* @param rightdistance the distance to be driven by the right wheel
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::drive_distance_mm(unsigned int leftdistance, unsigned int rightdistance){
|
|
// calls two subroutines for both sides
|
|
drive_distance_left_mm(leftdistance);
|
|
drive_distance_right_mm(rightdistance);
|
|
}
|
|
/** @brief tells the coderacer to drive a specified distance in mm for the left wheel.
|
|
* @param distance_mm a distance in mm
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::drive_distance_left_mm(int distance_mm){
|
|
distance_mm/=DISTANCE_PER_TICK_MM;
|
|
drive_ticks_left(distance_mm);
|
|
}
|
|
/** @brief tells the coderacer to drive a specified distance in mm for the right wheel.
|
|
* @param distance_mm a distance in mm
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::drive_distance_right_mm(int distance_mm){
|
|
distance_mm/=DISTANCE_PER_TICK_MM;
|
|
drive_ticks_right(distance_mm);
|
|
}
|
|
/** @brief checks if an obstacle is identified by either of the infrared sensors. returns true if that is the case for AT LEAST one of them
|
|
* @return true or false if an obstacle has been identified
|
|
*/
|
|
bool CodeRacerMKII::obstacle_check_left_right(){
|
|
//still needs a bit of fine tuning... currently, using digitalRead(), the infrared sensors can only "see" obstacles within 1cm in front of them.
|
|
if((true== obstacle_check_left()) || (true== obstacle_check_right()))
|
|
{
|
|
return true;
|
|
}
|
|
else {return false;}
|
|
}
|
|
/** @brief subroutine called by obstacle_check_left_right(). can be used seperately to check if an obstacle has been identified by the left infrared sensor.
|
|
* @return true or false if an obstacle has been identified
|
|
*/
|
|
bool CodeRacerMKII::obstacle_check_left(){
|
|
return obstacle_check(_infrared_sensor_left);
|
|
}
|
|
/** @brief subroutine called by obstacle_check_left_right(). can be used seperately to check if an obstacle has been identified by the left infrared sensor.
|
|
* @return true or false if an obstacle has been identified
|
|
*/
|
|
bool CodeRacerMKII::obstacle_check_right(){
|
|
return obstacle_check(_infrared_sensor_right);
|
|
}
|
|
/** @brief universal routine called by all obstacle_check routines. reads the pin the infrared sensor is connected at.
|
|
* @return true if voltage is present, false if not
|
|
*/
|
|
bool CodeRacerMKII::obstacle_check(uint8_t sensorpin) {
|
|
if (digitalRead(sensorpin)== HIGH) {
|
|
return true;
|
|
}
|
|
else {return false;}
|
|
}
|
|
|
|
/** @} */ // end of group lowerleveldrivesgetters
|
|
/** @} */ // end of group lowerleveldrives
|
|
|
|
|
|
//**************************************
|
|
//*** LED lower level control ***
|
|
//**************************************
|
|
/** @defgroup lowerlevelled Lower level LED methods
|
|
* @{
|
|
*/
|
|
/** @defgroup lowerlevelledmeths Methods
|
|
* @{
|
|
*/
|
|
|
|
/** @brief Sets all of the 4 LEDs to a ledstate (LEDON, LEDOFF)
|
|
* @param leftled set state of status left LED (most left yellow led)
|
|
* @param rightled set state of status stop LED (red led)
|
|
* @param stopled set state of status forward LED (green or blue led)
|
|
* @param frwrdled set state of status right LED (most right yellow led)
|
|
* @param blueled set state of blue LED
|
|
* @param whiteled set state of white LED
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::set_leds(ledstate leftled, ledstate rightled, ledstate stopled, ledstate frwrdled, ledstate blueled, ledstate whiteled) {
|
|
digitalWrite(_led_left_pin, leftled);
|
|
digitalWrite(_led_right_pin, rightled);
|
|
digitalWrite(_led_stop_pin, stopled);
|
|
digitalWrite(_led_frwd_pin, frwrdled);
|
|
digitalWrite(_led_blue_pin, blueled);
|
|
digitalWrite(_led_white_pin, whiteled);
|
|
}
|
|
|
|
/** @brief Sets all of the 4 LEDs to the same ledstate (LEDON, LEDOFF)
|
|
* @param alleds set state to all status LEDs
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::set_leds_all(ledstate alleds) {
|
|
digitalWrite(_led_left_pin, alleds);
|
|
digitalWrite(_led_right_pin, alleds);
|
|
digitalWrite(_led_blue_pin, alleds);
|
|
digitalWrite(_led_white_pin, alleds);
|
|
}
|
|
|
|
/** @brief Sets all of the 4 LEDs to the ledstate LEDOFF
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::set_leds_all_off() {
|
|
set_leds_all(LEDOFF);
|
|
}
|
|
|
|
/** @brief Sets all of the 4 LEDs to the ledstate LEDON
|
|
* @return nothing
|
|
*/
|
|
void CodeRacerMKII::set_leds_all_on() {
|
|
set_leds_all(LEDON);
|
|
}
|
|
|
|
int CodeRacerMKII::get_sweep_step_left()
|
|
{
|
|
return _servo_sweep_step_left;
|
|
}
|
|
|
|
int CodeRacerMKII::get_sweep_step_right()
|
|
{
|
|
return _servo_sweep_step_right;
|
|
}
|
|
|
|
void CodeRacerMKII::set_sweep_step_left(int stepleft)
|
|
{
|
|
_servo_sweep_step_left = stepleft;
|
|
}
|
|
void CodeRacerMKII::set_sweep_step_right(int stepright)
|
|
{
|
|
_servo_sweep_step_right = stepright;
|
|
}
|
|
|
|
void CodeRacerMKII::_get_calibration_factor()
|
|
{
|
|
_calibration_factor= NVS.getFloat("calfactor");
|
|
}
|
|
|
|
void CodeRacerMKII::_set_calibration_factor(float _factor)
|
|
{
|
|
NVS.setFloat("calfactor", _factor);
|
|
}
|
|
|
|
void CodeRacerMKII::calibrate_drives()
|
|
{
|
|
float smallest = 100;
|
|
float biggest = -1;
|
|
float sumfactor = 0;
|
|
int number_value = 0;
|
|
_calibration_factor = 1;
|
|
Coderacer.speed_settings(255,255);
|
|
Coderacer.drive_forward();
|
|
wait_ms(2000);
|
|
for(int value = 100; value <= 200; value = value + 20)
|
|
{
|
|
Coderacer.speed_settings(value, value);
|
|
Coderacer.drive_forward();
|
|
wait_ms(1000);
|
|
int startleft = Coderacer.show_left_stepcounter();
|
|
int startright = Coderacer.show_right_stepcounter();
|
|
|
|
wait_ms(10000);
|
|
Coderacer.stop_driving();
|
|
int left = Coderacer.show_left_stepcounter() - startleft;
|
|
int right = Coderacer.show_right_stepcounter() - startright;
|
|
|
|
if (left > 0 && right > 0)
|
|
{
|
|
float lefttoright = (float)left / (float)right;
|
|
number_value++;
|
|
sumfactor = sumfactor +lefttoright;
|
|
if (lefttoright < smallest)
|
|
smallest = lefttoright;
|
|
if (lefttoright > biggest)
|
|
biggest = lefttoright;
|
|
}
|
|
}
|
|
Coderacer.set_inactive();
|
|
sumfactor = sumfactor - smallest - biggest;
|
|
number_value = number_value - 2;
|
|
_factor = (float)sumfactor / (float)number_value;
|
|
_set_calibration_factor(_factor);
|
|
_get_calibration_factor();
|
|
}
|
|
|
|
/** @} */ // end of group lowerlevelledmets
|
|
/** @} */ // end of group lowerlevelled
|
|
|
|
|
|
//**************************************
|
|
//*** ISRs ***
|
|
//**************************************
|
|
//IRAM_ATTR is used to load the code into DRAM and not to Flash to make it faster - required for ISRs
|
|
|
|
/** @brief this ISR is called each time the left button is pressed, and toggles the Racer between active and inactive mode.
|
|
* @return nothing
|
|
*/
|
|
void IRAM_ATTR CodeRacerMKII::_set_button_state_left() {
|
|
if ((millis() - left_button_last_pressed_at_ms) > BUTTON_BOUNCING_TIME_MS) {
|
|
left_button_last_pressed_at_ms = millis(); // simplest debouncing - just wait ;-)
|
|
coderracer_activ = !coderracer_activ;
|
|
}
|
|
}
|
|
|
|
/** @brief this ISR is called each time the right button is pressed and increments a counter variable for it. THis variable has no use
|
|
* at the current moment, so feel free to use it in your main.
|
|
* @return nothing
|
|
*/
|
|
void IRAM_ATTR CodeRacerMKII::_set_button_state_right() {
|
|
if ((millis() - right_button_last_pressed_at_ms) > BUTTON_BOUNCING_TIME_MS) {
|
|
right_button_last_pressed_at_ms = millis();
|
|
right_button_pressed = !right_button_pressed;
|
|
right_button_count++;
|
|
}
|
|
|
|
}
|
|
|
|
/** @brief this routine is called each time the left PE barrier changes its state. it increments (and by that counts) the ticks driven by the left wheel and is also instructed to stop the racer if the stopcount set by drive_ticks() is reached using the stop_driving() routine
|
|
* @return nothing
|
|
*/
|
|
static void IRAM_ATTR _count_steps_left() {
|
|
//timeafterleft= millis();
|
|
//timebeforeleft = timebeforeleft == 0 ? timeafterleft : timebeforeleft;
|
|
left_step_counter++;
|
|
if (left_step_counter>=left_step_stopcount && left_step_stopcount != 0)
|
|
{
|
|
Coderacer.stop_driving();
|
|
left_step_stopcount= 0;
|
|
right_step_stopcount=0;
|
|
}
|
|
|
|
}
|
|
/** @brief this routine is called each time the left PE barrier changes its state. it increments (and by that counts) the ticks driven by the right wheel and is also instructed to stop the racer if the stopcount set by drive_ticks() is reached using the stop_driving() routine
|
|
* @return nothing
|
|
*/
|
|
static void IRAM_ATTR _count_steps_right() {
|
|
|
|
//timeafterright= millis();
|
|
//timebeforeright = timebeforeright == 0 ? timeafterright : timebeforeright;
|
|
right_step_counter++;
|
|
if (right_step_counter>=right_step_stopcount && right_step_stopcount != 0)
|
|
{
|
|
Coderacer.stop_driving();
|
|
left_step_stopcount=0;
|
|
right_step_stopcount=0;
|
|
}
|
|
|
|
}
|
|
/** @brief this routine is called each time the left infrared sensor changes its state. it sets the boolean variable obstacle_left_side to true/false and, if obstacle_stop is enabled, also stops the racer using stop_driving()
|
|
* @return nothing
|
|
*/
|
|
void IRAM_ATTR CodeRacerMKII::_stop_obstacle_left(){
|
|
|
|
if(HIGH== digitalRead(H_INFRARED_SENSOR_LEFT)){
|
|
if(true==obstacle_stop){
|
|
Coderacer.stop_driving();
|
|
}
|
|
obstacle_left_side=true;
|
|
}
|
|
else obstacle_left_side= false;
|
|
}
|
|
/** @brief this routine is called each time the left infrared sensor changes its state. it sets the boolean variable obstacle_left_side to true/false and, if obstacle_stop is enabled, also stops the racer using stop_driving()
|
|
* @return nothing
|
|
*/
|
|
void IRAM_ATTR CodeRacerMKII::_stop_obstacle_right(){
|
|
|
|
if(HIGH== digitalRead(H_INFRARED_SENSOR_RIGHT)){
|
|
if(1==obstacle_stop){
|
|
Coderacer.stop_driving();
|
|
}
|
|
obstacle_right_side=true;
|
|
}
|
|
else obstacle_right_side= false;
|
|
}
|
|
|
|
|
|
|
|
|
|
//**********************************************************************************************************************
|
|
//** Helper methods
|
|
//**********************************************************************************************************************
|
|
|
|
/** @brief an issue with delay() is, that it stops the processor of the ESP32 completely for the waiting time and with this the interrupt routines for the stepcounters. Wait_ms() basically does the same just without the
|
|
* freezing of the prozedures. PREFER OVER DELAY()!!
|
|
* @param waittime_ms the time you want to wait- in ms
|
|
* @return nothing
|
|
*/
|
|
void wait_ms(long waittime_ms)
|
|
{
|
|
long started_ms = millis();
|
|
while(millis()-started_ms < waittime_ms){}
|
|
}
|
|
|
|
//**********************************************************************************************************************
|
|
//** Status **
|
|
//**********************************************************************************************************************
|
|
|
|
void CodeRacerMKII::battery_test()
|
|
{
|
|
uint32_t voltage = analogReadMilliVolts(26);
|
|
if(voltage < 1801 && voltage > 1500)
|
|
Coderacer.set_leds(LEDOFF, LEDOFF, LEDOFF, LEDOFF, LEDOFF, LEDON);
|
|
else if(voltage < 1500)
|
|
{
|
|
Coderacer.set_leds(LEDOFF, LEDOFF, LEDOFF, LEDOFF, LEDOFF, LEDON);
|
|
wait_ms(500);
|
|
Coderacer.set_leds(LEDOFF, LEDOFF, LEDOFF, LEDOFF, LEDOFF, LEDOFF);
|
|
}
|
|
else
|
|
Coderacer.set_leds(LEDOFF, LEDOFF, LEDOFF, LEDOFF, LEDOFF, LEDOFF);
|
|
} |