diff --git a/CodeRacer_MKII/CodeRacer_MKII.cpp b/CodeRacer_MKII/CodeRacer_MKII.cpp index ef83ab4..de6e895 100644 --- a/CodeRacer_MKII/CodeRacer_MKII.cpp +++ b/CodeRacer_MKII/CodeRacer_MKII.cpp @@ -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!

"); @@ -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); +} \ No newline at end of file diff --git a/CodeRacer_MKII/CodeRacer_MKII.h b/CodeRacer_MKII/CodeRacer_MKII.h index b88c121..5ad5c73 100644 --- a/CodeRacer_MKII/CodeRacer_MKII.h +++ b/CodeRacer_MKII/CodeRacer_MKII.h @@ -6,6 +6,7 @@ #include "Webserver.h" #include // support for vectors #include +#include #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();