diff --git a/vsode/coderacer_mkII/lib/CodeRacer_MKII/CodeRacer_MKII.cpp b/vsode/coderacer_mkII/lib/CodeRacer_MKII/CodeRacer_MKII.cpp index 3dce5a4..9e7b15b 100644 --- a/vsode/coderacer_mkII/lib/CodeRacer_MKII/CodeRacer_MKII.cpp +++ b/vsode/coderacer_mkII/lib/CodeRacer_MKII/CodeRacer_MKII.cpp @@ -11,20 +11,22 @@ 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= 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_stop= true; 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 @@ -102,6 +104,7 @@ CodeRacerMKII::CodeRacerMKII(uint8_t button_pin_left , uint8_t button_pin_right, _led_right_pin = led_right_pin; _led_blue_pin = led_blue_pin; _led_white_pin= led_white_pin; + } /** @brief Initialisation of all attributes and settings of the coderacer. Defaults are taken from the header file. @@ -191,7 +194,7 @@ void CodeRacerMKII::begin() { 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; @@ -204,7 +207,11 @@ void CodeRacerMKII::begin() { randomSeed(analogRead(0)); //fun stuff - coderacer_fun_enabled = false; + coderacer_fun_enabled = true; + servo_set_to_center(); + + //the Debug string for webserver + } //************************************** @@ -217,7 +224,7 @@ void CodeRacerMKII::begin() { * @{ */ -/** @brief Stops the racer and sets status LEDs. +/** @brief Stops the racer. * @return nothing */ void CodeRacerMKII::stop_driving() { @@ -225,7 +232,7 @@ void CodeRacerMKII::stop_driving() { _drive = false; global_stop_driving(); //set_drives_stop_left_right(); - //set_leds_left_stop_frwd_right(LEDOFF, LEDOFF, LEDON, LEDOFF, LEDOFF, LEDOFF); + //set_leds(LEDOFF, LEDOFF, LEDON, LEDOFF, LEDOFF, LEDOFF); } /** @brief Sets the speed and the directions of both drives so that it will move forward. @@ -249,10 +256,20 @@ 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); - set_leds_left_stop_frwd_right(LEDOFF, LEDOFF, LEDOFF, LEDON, LEDOFF, LEDOFF); + 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. * @@ -275,11 +292,22 @@ 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); - set_leds_left_stop_frwd_right(LEDOFF, LEDOFF, LEDON, LEDOFF, LEDOFF, LEDOFF); + 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. @@ -291,7 +319,7 @@ 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 the specified time in ms and with the internally stored 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 @@ -313,17 +341,19 @@ 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_left_stop_frwd_right(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); + wifi_printf("Turning left!
"); // wait set delay for the timed turn _turn_left_for_ms = turn_for_ms; - delay(_turn_left_for_ms); + wait_ms(_turn_left_for_ms); // stop drives again - set_drives_stop_left_right(); + global_stop_driving(); } -/** @brief Will turn the racer to the right for the internally stroe time in ms and with the internally stored speed. +/** @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. @@ -356,17 +386,18 @@ 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_left_stop_frwd_right(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!
");
// wait set delay for the timed turn
_turn_right_for_ms = turn_for_ms;
- delay(_turn_right_for_ms);
+ wait_ms(_turn_right_for_ms);
// stop drives again
- set_drives_stop_left_right();
+ global_stop_driving();
}
-/** @brief Enables to stopp the racer if during a distance measurement the measured distance is smaller then the internally stored minimal distance.
+/** @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.
@@ -430,6 +461,22 @@ bool CodeRacerMKII::start_stop() {
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
* @{
@@ -516,8 +563,7 @@ unsigned int CodeRacerMKII::button_count() {
* @return driven_distance
*/
unsigned int CodeRacerMKII::show_distance_mm(){
- int driven_distance;
- driven_distance= left_step_counter*DISTANCE_PER_TICK_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
@@ -601,25 +647,25 @@ void CodeRacerMKII::kitt()
_last_led_on = _last_led_on + _led_count;
switch (_last_led_on) {
case 0:
- set_leds_left_stop_frwd_right(LEDOFF, LEDOFF, LEDOFF, LEDOFF, LEDOFF, LEDOFF);
+ set_leds(LEDOFF, LEDOFF, LEDOFF, LEDOFF, LEDOFF, LEDOFF);
break;
case 1:
- set_leds_left_stop_frwd_right(LEDON, LEDOFF, LEDOFF, LEDOFF, LEDOFF, LEDOFF);
+ set_leds(LEDON, LEDOFF, LEDOFF, LEDOFF, LEDOFF, LEDOFF);
break;
case 2:
- set_leds_left_stop_frwd_right(LEDOFF, LEDON, LEDOFF, LEDOFF, LEDOFF, LEDOFF);
+ set_leds(LEDOFF, LEDON, LEDOFF, LEDOFF, LEDOFF, LEDOFF);
break;
case 3:
- set_leds_left_stop_frwd_right(LEDOFF, LEDOFF, LEDON, LEDOFF, LEDOFF, LEDOFF);
+ set_leds(LEDOFF, LEDOFF, LEDON, LEDOFF, LEDOFF, LEDOFF);
break;
case 4:
- set_leds_left_stop_frwd_right(LEDOFF, LEDOFF, LEDOFF, LEDON, LEDOFF, LEDOFF);
+ set_leds(LEDOFF, LEDOFF, LEDOFF, LEDON, LEDOFF, LEDOFF);
break;
case 5:
- set_leds_left_stop_frwd_right(LEDOFF, LEDOFF, LEDOFF, LEDOFF, LEDON, LEDOFF);
+ set_leds(LEDOFF, LEDOFF, LEDOFF, LEDOFF, LEDON, LEDOFF);
break;
case 6:
- set_leds_left_stop_frwd_right(LEDOFF, LEDOFF, LEDOFF, LEDOFF, LEDOFF, LEDON);
+ set_leds(LEDOFF, LEDOFF, LEDOFF, LEDOFF, LEDOFF, LEDON);
}
}
}
@@ -848,7 +894,7 @@ uint8_t CodeRacerMKII::servo_set_position_wait(uint8_t position)
{
_servo_sweep = false;
unsigned long wait_time_ms = _servo_set_position(position);
- delay(wait_time_ms);
+ wait_ms(wait_time_ms);
return(_servo_position);
}
@@ -1165,14 +1211,19 @@ void CodeRacerMKII::drives_settings(uint8_t drive_left_speed, uint8_t drive_righ
_turn_left_for_ms = turn_left_for_ms;
_turn_right_for_ms = turn_right_for_ms;
}
-
-/** @brief Stopps both drives
+/** @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::set_drives_stop_left_right() {
- set_drive_left_state(DRIVESTOP);
- set_drive_right_state(DRIVESTOP);
-}
+void CodeRacerMKII::Reset_Stats(){
+ global_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.");
+}
/** @brief Sets both of the drives to a specified drivestate (DRIVESTOP, DRIVEFRWD, DRIVEBACK)
* @param stateleft drivestate to set for the left side drive
@@ -1336,7 +1387,7 @@ void CodeRacerMKII::drive_ticks(unsigned int tickleft, unsigned int tickright){
* @return nothing
*/
void CodeRacerMKII::turn_degree(int degree){
- // sets thhe drive states for both wheels and by that the direction the racer will turn.
+ // 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);
@@ -1346,13 +1397,15 @@ void CodeRacerMKII::turn_degree(int degree){
set_drives_states_left_right(DRIVEBACK, DRIVEFRWD);
degree= abs(degree);
}
- else set_drives_states_left_right(DRIVESTOP, DRIVESTOP);
+ 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
@@ -1402,6 +1455,7 @@ void CodeRacerMKII::drive_distance_right_mm(int distance_mm){
* @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;
@@ -1424,7 +1478,7 @@ bool CodeRacerMKII::obstacle_check_right(){
* @return true if voltage is present, false if not
*/
bool CodeRacerMKII::obstacle_check(uint8_t sensorpin) {
- if (true== digitalRead(sensorpin)) {
+ if (digitalRead(sensorpin)== HIGH) {
return true;
}
else {return false;}
@@ -1453,7 +1507,7 @@ bool CodeRacerMKII::obstacle_check(uint8_t sensorpin) {
* @param whiteled set state of white LED
* @return nothing
*/
-void CodeRacerMKII::set_leds_left_stop_frwd_right(ledstate leftled, ledstate rightled, ledstate stopled, ledstate frwrdled, ledstate blueled, ledstate whiteled) {
+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);
@@ -1520,6 +1574,7 @@ static void IRAM_ATTR global_stop_driving(){
{
ledcWrite(DRIVE_PWM_LEFT_CHANNEL, 0);
}
+ debug_message+="Stopped.";
digitalWrite(H_LED_RIGHT_PIN, LOW);
digitalWrite(H_LED_LEFT_PIN, LOW);
digitalWrite(H_LED_STOP_PIN, HIGH);
@@ -1527,13 +1582,19 @@ static void IRAM_ATTR global_stop_driving(){
digitalWrite(H_LED_BLUE_PIN, LOW);
digitalWrite(H_LED_WHITE_PIN, LOW);
_drive= false;
+ digitalWrite(H_LED_RIGHT_PIN, LOW);
+ digitalWrite(H_LED_LEFT_PIN, LOW);
+ digitalWrite(H_LED_STOP_PIN, LOW);
+ digitalWrite(H_LED_FRWD_PIN, LOW);
+ digitalWrite(H_LED_BLUE_PIN, LOW);
+ digitalWrite(H_LED_WHITE_PIN, LOW);
}
/** @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 global_stop_driving() routine
* @return nothing
*/
static void IRAM_ATTR _count_steps_left() {
- timeafterleft= millis();
- timebeforeleft = timebeforeleft == 0 ? timeafterleft : timebeforeleft;
+ //timeafterleft= millis();
+ //timebeforeleft = timebeforeleft == 0 ? timeafterleft : timebeforeleft;
left_step_counter++;
if (left_step_counter==left_step_stopcount)
{
@@ -1541,13 +1602,15 @@ static void IRAM_ATTR _count_steps_left() {
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 global_stop_driving() routine
* @return nothing
*/
static void IRAM_ATTR _count_steps_right() {
- timeafterright= millis();
- timebeforeright = timebeforeright == 0 ? timeafterright : timebeforeright;
+
+ //timeafterright= millis();
+ //timebeforeright = timebeforeright == 0 ? timeafterright : timebeforeright;
right_step_counter++;
if (right_step_counter==right_step_stopcount)
{
@@ -1556,13 +1619,15 @@ static void IRAM_ATTR _count_steps_right() {
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 global_stop_driving()
* @return nothing
*/
void IRAM_ATTR CodeRacerMKII::_stop_obstacle_left(){
- if(HIGH== digitalRead(H_INFRARED_SENSOR_LEFT)){
- if(1==obstacle_stop){
+
+ if(LOW== digitalRead(H_INFRARED_SENSOR_LEFT)){
+ if(true==obstacle_stop){
global_stop_driving();
}
obstacle_left_side=true;
@@ -1573,6 +1638,7 @@ void IRAM_ATTR CodeRacerMKII::_stop_obstacle_left(){
* @return nothing
*/
void IRAM_ATTR CodeRacerMKII::_stop_obstacle_right(){
+
if(HIGH== digitalRead(H_INFRARED_SENSOR_RIGHT)){
if(1==obstacle_stop){
global_stop_driving();
@@ -1589,8 +1655,14 @@ void IRAM_ATTR CodeRacerMKII::_stop_obstacle_right(){
//** 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){}
}
+
diff --git a/vsode/coderacer_mkII/lib/CodeRacer_MKII/CodeRacer_MKII.h b/vsode/coderacer_mkII/lib/CodeRacer_MKII/CodeRacer_MKII.h
index e1ce099..cfe1f98 100644
--- a/vsode/coderacer_mkII/lib/CodeRacer_MKII/CodeRacer_MKII.h
+++ b/vsode/coderacer_mkII/lib/CodeRacer_MKII/CodeRacer_MKII.h
@@ -3,6 +3,7 @@
#include
+
+
+
+
+
+
+
+
+
+
+
+
+ Welcome to the CodeRacer Webserver!
+
+
+
+ Step Counters Speed of Drives Servo- Information Driven Distance AJAX Information
+ 0 0 0 0 0
+ 0 0 0 0 0
GPIO 32 - State " + output32State ); - // If the output27State is off, it displays the ON button - if (output32State=="off") { - client.println(""); - } else { - client.println(""); - } - - client.println("GPIO 33 - State " + output33State ); - // If the output27State is off, it displays the ON button - if (output33State=="off") { - client.println("
"); - } else { - client.println(""); - } - // Display current state, and ON/OFF buttons for GPIO 26 - client.println("GPIO 25 - State " + output25State ); - // If the output26State is off, it displays the ON button - if (output25State=="off") { - client.println(""); - } else { - client.println(""); - } - - // Display current state, and ON/OFF buttons for GPIO 27 - client.println("GPIO 27 - State " + output27State ); - // If the output27State is off, it displays the ON button - if (output27State=="off") { - client.println("
"); - } else { - client.println(""); - } - - client.println("", Distance); - client.println("