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

127
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_3= switch_3;
_switch_4= switch_4;
}
/** @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_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;
@ -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_speed_left_right(left_speed, right_speed);
wifi_printf("Driving forward...");
set_leds(LEDOFF, LEDOFF, LEDOFF, LEDON, LEDOFF, LEDOFF);
//set_leds(LEDOFF, LEDOFF, LEDOFF, LEDON, LEDOFF, LEDOFF);
_drive = true;
_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_speed_left_right(left_speed, right_speed);
wifi_printf("Driving Backwards...");
set_leds(LEDOFF, LEDOFF, LEDOFF, LEDOFF, LEDON, LEDOFF);
//set_leds(LEDOFF, LEDOFF, LEDOFF, LEDOFF, LEDON, LEDOFF);
_drive = true;
_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)
{
_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_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)
{
_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_speed_left_right(left_speed, right_speed);
wifi_printf("Turning right! <p>");
@ -460,11 +465,11 @@ bool CodeRacerMKII::start_stop() {
if (_coderracer_activ != coderracer_activ) {
_coderracer_activ = coderracer_activ;
if (true == _coderracer_activ) {
set_leds_all_off();
//set_leds_all_off();
}
else {
stop_driving();
set_leds_all_off();
//set_leds_all_off();
servo_set_to_center();
_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);
if ((position >= servo_sweep_left_pos) || (position >= SERVO_MAX_POSITION)) {
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)) {
position = servo_sweep_right_pos;
_servo_sweep_step = SERVO_SWEEP_TO_LEFT_STEP;
_servo_sweep_step = _servo_sweep_step_left;
}
_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_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
@ -1366,9 +1370,15 @@ void CodeRacerMKII::set_drives_speed_left_right(uint8_t speedleft, uint8_t speed
* @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
@ -1376,6 +1386,10 @@ void CodeRacerMKII::set_drive_left_speed(uint8_t speed) {
* @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);
}
@ -1577,8 +1591,6 @@ void CodeRacerMKII::set_leds(ledstate leftled, ledstate rightled, ledstate stopl
void CodeRacerMKII::set_leds_all(ledstate alleds) {
digitalWrite(_led_left_pin, alleds);
digitalWrite(_led_right_pin, alleds);
digitalWrite(_led_stop_pin, alleds);
digitalWrite(_led_frwd_pin, alleds);
digitalWrite(_led_blue_pin, alleds);
digitalWrite(_led_white_pin, alleds);
}
@ -1597,6 +1609,77 @@ 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
@ -1636,7 +1719,7 @@ static void IRAM_ATTR _count_steps_left() {
//timeafterleft= millis();
//timebeforeleft = timebeforeleft == 0 ? timeafterleft : timebeforeleft;
left_step_counter++;
if (left_step_counter==left_step_stopcount)
if (left_step_counter>=left_step_stopcount && left_step_stopcount != 0)
{
Coderacer.stop_driving();
left_step_stopcount= 0;
@ -1652,7 +1735,7 @@ static void IRAM_ATTR _count_steps_right() {
//timeafterright= millis();
//timebeforeright = timebeforeright == 0 ? timeafterright : timebeforeright;
right_step_counter++;
if (right_step_counter==right_step_stopcount)
if (right_step_counter>=right_step_stopcount && right_step_stopcount != 0)
{
Coderacer.stop_driving();
left_step_stopcount=0;
@ -1705,3 +1788,21 @@ void wait_ms(long 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 <vector> // support for vectors
#include <string>
#include <TridentTD_ESP32NVS.h>
#if !defined(CONFIG_BT_ENABLED) || !defined(CONFIG_BLUEDROID_ENABLED)
#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_MAX_POSITION 170 // maximum 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 -----
#define H_LEFT_BARRIER_PIN 34
@ -145,6 +146,8 @@ class CodeRacerMKII {
//servo variables
int8_t _servo_sweep_step;
int8_t _servo_sweep_step_left;
int8_t _servo_sweep_step_right;
uint8_t _servo_position;
unsigned long _servo_position_set_at_ms;
unsigned long _servo_position_eta_in_ms;
@ -154,6 +157,8 @@ class CodeRacerMKII {
uint8_t _drive_right_speed;
unsigned long _turn_left_for_ms;
unsigned long _turn_right_for_ms;
float _calibration_factor;
float _factor;
// ultrasonic variables
bool _coderacer_stopped_at_min_distance;
@ -182,6 +187,8 @@ class CodeRacerMKII {
static void _set_button_state_right();
static void _stop_obstacle_left();
static void _stop_obstacle_right();
void _get_calibration_factor();
void _set_calibration_factor(float _factor);
void _analog_write(uint8_t pin, uint8_t speed);
unsigned long _servo_set_position(uint8_t position);
@ -212,6 +219,9 @@ class CodeRacerMKII {
void begin();
//status
void battery_test();
// getters
bool is_active();
bool is_driving();
@ -310,6 +320,7 @@ class CodeRacerMKII {
bool obstacle_check_right();
bool obstacle_check_left();
bool obstacle_check (uint8_t sensorpin);
void calibrate_drives();
// Ultrasonic sensor
unsigned long usonic_measure_cm();
@ -331,6 +342,10 @@ class CodeRacerMKII {
void servo_set_to_left();
void servo_set_to_center();
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
void kitt();