Added Batterycheck, Calibration and sweep step setters and getters
This commit is contained in:
Jens Noack
parent
54ad24ec44
commit
f2e6de783f
|
|
@ -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) {
|
||||
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.
|
||||
*
|
||||
* 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);
|
||||
}
|
||||
|
|
@ -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);
|
||||
|
||||
|
|
@ -211,6 +218,9 @@ class CodeRacerMKII {
|
|||
void set_active();
|
||||
|
||||
void begin();
|
||||
|
||||
//status
|
||||
void battery_test();
|
||||
|
||||
// getters
|
||||
bool is_active();
|
||||
|
|
@ -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();
|
||||
|
|
|
|||
Loading…
Reference in New Issue