coderacer/library
2
0
Fork 0
Code Issues Pull requests Projects Releases 2 Packages Wiki Activity

Added Batterycheck, Calibration and sweep step setters and getters

Browse source
This commit is contained in:
Jens Noack 2021-07-23 10:27:36 +02:00
parent 54ad24ec44
commit f2e6de783f
2 changed files with 131 additions and 15 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

129
CodeRacer_MKII/CodeRacer_MKII.cpp
View file

@ -115,6 +115,7 @@ CodeRacerMKII::CodeRacerMKII(uint8_t button_pin_left , uint8_t button_pin_right,
_switch_2= switch_2; _switch_2= switch_2;
_switch_3= switch_3; _switch_3= switch_3;
_switch_4= switch_4; _switch_4= switch_4;
} }
/** @brief Initialisation of all attributes and settings of the coderacer. Defaults are taken from the header file. /** @brief Initialisation of all attributes and settings of the coderacer. Defaults are taken from the header file.
@ -138,9 +139,13 @@ void CodeRacerMKII::begin() {
servo_sweep_right_pos = H_SERVO_SWEEP_RIGHT_POS; servo_sweep_right_pos = H_SERVO_SWEEP_RIGHT_POS;
_servo_position = servo_center_pos; _servo_position = servo_center_pos;
_servo_sweep_step = SERVO_SWEEP_TO_LEFT_STEP; _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_set_at_ms = millis();
_servo_position_eta_in_ms = 0; _servo_position_eta_in_ms = 0;
NVS.begin();
_get_calibration_factor();
_drive_left_speed = H_DRIVE_LEFT_SPEED; _drive_left_speed = H_DRIVE_LEFT_SPEED;
_drive_right_speed = H_DRIVE_RIGHT_SPEED; _drive_right_speed = H_DRIVE_RIGHT_SPEED;
_turn_left_for_ms = H_RACER_TURN_LEFT_FOR_MS; _turn_left_for_ms = H_RACER_TURN_LEFT_FOR_MS;
@ -274,7 +279,7 @@ void CodeRacerMKII::drive_forward(uint8_t left_speed, uint8_t right_speed)
set_drives_states_left_right(DRIVEFRWD, DRIVEFRWD); set_drives_states_left_right(DRIVEFRWD, DRIVEFRWD);
set_drives_speed_left_right(left_speed, right_speed); set_drives_speed_left_right(left_speed, right_speed);
wifi_printf("Driving forward..."); wifi_printf("Driving forward...");
set_leds(LEDOFF, LEDOFF, LEDOFF, LEDON, LEDOFF, LEDOFF); //set_leds(LEDOFF, LEDOFF, LEDOFF, LEDON, LEDOFF, LEDOFF);
_drive = true; _drive = true;
_drive_set_at_ms = millis(); _drive_set_at_ms = millis();
} }
@ -310,7 +315,7 @@ void CodeRacerMKII::drive_backward(uint8_t left_speed, uint8_t right_speed)
set_drives_states_left_right(DRIVEBACK, DRIVEBACK); set_drives_states_left_right(DRIVEBACK, DRIVEBACK);
set_drives_speed_left_right(left_speed, right_speed); set_drives_speed_left_right(left_speed, right_speed);
wifi_printf("Driving Backwards..."); wifi_printf("Driving Backwards...");
set_leds(LEDOFF, LEDOFF, LEDOFF, LEDOFF, LEDON, LEDOFF); //set_leds(LEDOFF, LEDOFF, LEDOFF, LEDOFF, LEDON, LEDOFF);
_drive = true; _drive = true;
_drive_set_at_ms = millis(); _drive_set_at_ms = millis();
} }
@ -358,7 +363,7 @@ void CodeRacerMKII::turn_left(unsigned long turn_for_ms)
void CodeRacerMKII::turn_left(unsigned long turn_for_ms, uint8_t left_speed, uint8_t right_speed) void CodeRacerMKII::turn_left(unsigned long turn_for_ms, uint8_t left_speed, uint8_t right_speed)
{ {
_drive = false; _drive = false;
set_leds(LEDON, LEDOFF, LEDOFF, LEDOFF, LEDOFF, LEDOFF); // LEDs setzen //set_leds(LEDON, LEDOFF, LEDOFF, LEDOFF, LEDOFF, LEDOFF); // LEDs setzen
set_drives_states_left_right(DRIVEBACK, DRIVEFRWD); set_drives_states_left_right(DRIVEBACK, DRIVEFRWD);
set_drives_speed_left_right(left_speed, right_speed); set_drives_speed_left_right(left_speed, right_speed);
@ -403,7 +408,7 @@ void CodeRacerMKII::turn_right(unsigned long turn_for_ms)
void CodeRacerMKII::turn_right(unsigned long turn_for_ms, uint8_t left_speed, uint8_t right_speed) void CodeRacerMKII::turn_right(unsigned long turn_for_ms, uint8_t left_speed, uint8_t right_speed)
{ {
_drive = false; _drive = false;
set_leds(LEDOFF, LEDON, LEDOFF, LEDOFF, LEDOFF, LEDOFF); // LEDs setzen //set_leds(LEDOFF, LEDON, LEDOFF, LEDOFF, LEDOFF, LEDOFF); // LEDs setzen
set_drives_states_left_right(DRIVEFRWD, DRIVEBACK); set_drives_states_left_right(DRIVEFRWD, DRIVEBACK);
set_drives_speed_left_right(left_speed, right_speed); set_drives_speed_left_right(left_speed, right_speed);
wifi_printf("Turning right! <p>"); wifi_printf("Turning right! <p>");
@ -460,11 +465,11 @@ bool CodeRacerMKII::start_stop() {
if (_coderracer_activ != coderracer_activ) { if (_coderracer_activ != coderracer_activ) {
_coderracer_activ = coderracer_activ; _coderracer_activ = coderracer_activ;
if (true == _coderracer_activ) { if (true == _coderracer_activ) {
set_leds_all_off(); //set_leds_all_off();
} }
else { else {
stop_driving(); stop_driving();
set_leds_all_off(); //set_leds_all_off();
servo_set_to_center(); servo_set_to_center();
_servo_look_around_at_ms = millis() + random(20000, 120000); _servo_look_around_at_ms = millis() + random(20000, 120000);
} }
@ -899,11 +904,11 @@ void CodeRacerMKII::servo_sweep()
//sprintf(_debugmsg,"[%s] current position=%ld newpostion=%ld", __func__, _servo_position, position); //sprintf(_debugmsg,"[%s] current position=%ld newpostion=%ld", __func__, _servo_position, position);
if ((position >= servo_sweep_left_pos) || (position >= SERVO_MAX_POSITION)) { if ((position >= servo_sweep_left_pos) || (position >= SERVO_MAX_POSITION)) {
position = servo_sweep_left_pos; position = servo_sweep_left_pos;
_servo_sweep_step = SERVO_SWEEP_TO_RIGHT_STEP; _servo_sweep_step = _servo_sweep_step_right;
} }
if ((position <= servo_sweep_right_pos) || (position <= SERVO_MIN_POSITION)) { if ((position <= servo_sweep_right_pos) || (position <= SERVO_MIN_POSITION)) {
position = servo_sweep_right_pos; position = servo_sweep_right_pos;
_servo_sweep_step = SERVO_SWEEP_TO_LEFT_STEP; _servo_sweep_step = _servo_sweep_step_left;
} }
_servo_set_position(position); _servo_set_position(position);
} }
@ -1245,7 +1250,6 @@ void CodeRacerMKII::speed_settings(uint8_t drive_left_speed, uint8_t drive_right
_drive_left_speed = drive_left_speed; _drive_left_speed = drive_left_speed;
_drive_right_speed = drive_right_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. /** @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_left_speed Speed of the left side drive
* @param drive_right_speed Speed of the right side drive * @param drive_right_speed Speed of the right side drive
@ -1366,9 +1370,15 @@ void CodeRacerMKII::set_drives_speed_left_right(uint8_t speedleft, uint8_t speed
* @return nothing * @return nothing
*/ */
void CodeRacerMKII::set_drive_left_speed(uint8_t speed) { void CodeRacerMKII::set_drive_left_speed(uint8_t speed) {
set_drive_speed(speed, _drive_left_enable_pin); 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. /** @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 * 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
@ -1376,6 +1386,10 @@ void CodeRacerMKII::set_drive_left_speed(uint8_t speed) {
* @return nothing * @return nothing
*/ */
void CodeRacerMKII::set_drive_right_speed(uint8_t speed) { 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); set_drive_speed(speed, _drive_right_enable_pin);
} }
@ -1577,8 +1591,6 @@ void CodeRacerMKII::set_leds(ledstate leftled, ledstate rightled, ledstate stopl
void CodeRacerMKII::set_leds_all(ledstate alleds) { void CodeRacerMKII::set_leds_all(ledstate alleds) {
digitalWrite(_led_left_pin, alleds); digitalWrite(_led_left_pin, alleds);
digitalWrite(_led_right_pin, alleds); digitalWrite(_led_right_pin, alleds);
digitalWrite(_led_stop_pin, alleds);
digitalWrite(_led_frwd_pin, alleds);
digitalWrite(_led_blue_pin, alleds); digitalWrite(_led_blue_pin, alleds);
digitalWrite(_led_white_pin, alleds); digitalWrite(_led_white_pin, alleds);
} }
@ -1597,6 +1609,77 @@ void CodeRacerMKII::set_leds_all_on() {
set_leds_all(LEDON); 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 lowerlevelledmets
/** @} */ // end of group lowerlevelled /** @} */ // end of group lowerlevelled
@ -1636,7 +1719,7 @@ static void IRAM_ATTR _count_steps_left() {
//timeafterleft= millis(); //timeafterleft= millis();
//timebeforeleft = timebeforeleft == 0 ? timeafterleft : timebeforeleft; //timebeforeleft = timebeforeleft == 0 ? timeafterleft : timebeforeleft;
left_step_counter++; left_step_counter++;
if (left_step_counter==left_step_stopcount) if (left_step_counter>=left_step_stopcount && left_step_stopcount != 0)
{ {
Coderacer.stop_driving(); Coderacer.stop_driving();
left_step_stopcount= 0; left_step_stopcount= 0;
@ -1652,7 +1735,7 @@ static void IRAM_ATTR _count_steps_right() {
//timeafterright= millis(); //timeafterright= millis();
//timebeforeright = timebeforeright == 0 ? timeafterright : timebeforeright; //timebeforeright = timebeforeright == 0 ? timeafterright : timebeforeright;
right_step_counter++; right_step_counter++;
if (right_step_counter==right_step_stopcount) if (right_step_counter>=right_step_stopcount && right_step_stopcount != 0)
{ {
Coderacer.stop_driving(); Coderacer.stop_driving();
left_step_stopcount=0; left_step_stopcount=0;
@ -1705,3 +1788,21 @@ void wait_ms(long waittime_ms)
while(millis()-started_ms < waittime_ms){} 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);
}

17
CodeRacer_MKII/CodeRacer_MKII.h
View file

@ -6,6 +6,7 @@
#include "Webserver.h" #include "Webserver.h"
#include <vector> // support for vectors #include <vector> // support for vectors
#include <string> #include <string>
#include <TridentTD_ESP32NVS.h>
#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED) #if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
#error Bluetooth is not enabled! Please run `make menuconfig` to and enable it #error Bluetooth is not enabled! Please run `make menuconfig` to and enable it
@ -36,7 +37,7 @@
#define SERVO_SWEEP_MS 10 // duration of time betwee to sweep steps #define SERVO_SWEEP_MS 10 // duration of time betwee to sweep steps
#define SERVO_MAX_POSITION 170 // maximum servo position #define SERVO_MAX_POSITION 170 // maximum servo position
#define SERVO_MIN_POSITION 10 // minimum servo position #define SERVO_MIN_POSITION 10 // minimum servo position
#define SERVO_SET_1TICK_POSITION_DELAY_MS 3 // minimum duration of time between two servo steps #define SERVO_SET_1TICK_POSITION_DELAY_MS 5 // minimum duration of time between two servo steps
//----- PE- barrier ----- //----- PE- barrier -----
#define H_LEFT_BARRIER_PIN 34 #define H_LEFT_BARRIER_PIN 34
@ -145,6 +146,8 @@ class CodeRacerMKII {
//servo variables //servo variables
int8_t _servo_sweep_step; int8_t _servo_sweep_step;
int8_t _servo_sweep_step_left;
int8_t _servo_sweep_step_right;
uint8_t _servo_position; uint8_t _servo_position;
unsigned long _servo_position_set_at_ms; unsigned long _servo_position_set_at_ms;
unsigned long _servo_position_eta_in_ms; unsigned long _servo_position_eta_in_ms;
@ -154,6 +157,8 @@ class CodeRacerMKII {
uint8_t _drive_right_speed; uint8_t _drive_right_speed;
unsigned long _turn_left_for_ms; unsigned long _turn_left_for_ms;
unsigned long _turn_right_for_ms; unsigned long _turn_right_for_ms;
float _calibration_factor;
float _factor;
// ultrasonic variables // ultrasonic variables
bool _coderacer_stopped_at_min_distance; bool _coderacer_stopped_at_min_distance;
@ -182,6 +187,8 @@ class CodeRacerMKII {
static void _set_button_state_right(); static void _set_button_state_right();
static void _stop_obstacle_left(); static void _stop_obstacle_left();
static void _stop_obstacle_right(); static void _stop_obstacle_right();
void _get_calibration_factor();
void _set_calibration_factor(float _factor);
void _analog_write(uint8_t pin, uint8_t speed); void _analog_write(uint8_t pin, uint8_t speed);
unsigned long _servo_set_position(uint8_t position); unsigned long _servo_set_position(uint8_t position);
@ -212,6 +219,9 @@ class CodeRacerMKII {
void begin(); void begin();
//status
void battery_test();
// getters // getters
bool is_active(); bool is_active();
bool is_driving(); bool is_driving();
@ -310,6 +320,7 @@ class CodeRacerMKII {
bool obstacle_check_right(); bool obstacle_check_right();
bool obstacle_check_left(); bool obstacle_check_left();
bool obstacle_check (uint8_t sensorpin); bool obstacle_check (uint8_t sensorpin);
void calibrate_drives();
// Ultrasonic sensor // Ultrasonic sensor
unsigned long usonic_measure_cm(); unsigned long usonic_measure_cm();
@ -331,6 +342,10 @@ class CodeRacerMKII {
void servo_set_to_left(); void servo_set_to_left();
void servo_set_to_center(); void servo_set_to_center();
void servo_sweep(); void servo_sweep();
int get_sweep_step_left();
int get_sweep_step_right();
void set_sweep_step_right(int stepright);
void set_sweep_step_left(int stepleft);
// just for fun // just for fun
void kitt(); void kitt();