From e0db522d10518ff051a46558c97e1ac8481d73ff Mon Sep 17 00:00:00 2001 From: jglueck Date: Fri, 3 Apr 2020 12:36:16 +0200 Subject: [PATCH] New MKII update with implementation of step- counter and some tweaks. --- .../lib/CodeRacer_MKII/CodeRacer_MKII.cpp | 156 ++++++++++++++++-- .../lib/CodeRacer_MKII/CodeRacer_MKII.h | 36 +++- vsode/coderacer_mkII/src/coderacer_main.cpp | 29 +--- 3 files changed, 186 insertions(+), 35 deletions(-) diff --git a/vsode/coderacer_mkII/lib/CodeRacer_MKII/CodeRacer_MKII.cpp b/vsode/coderacer_mkII/lib/CodeRacer_MKII/CodeRacer_MKII.cpp index 1638dcc..cafdd05 100644 --- a/vsode/coderacer_mkII/lib/CodeRacer_MKII/CodeRacer_MKII.cpp +++ b/vsode/coderacer_mkII/lib/CodeRacer_MKII/CodeRacer_MKII.cpp @@ -11,6 +11,8 @@ CodeRacerMKII::CodeRacerMKII() _button_pin_left = H_BUTTON_PIN_LEFT; _button_pin_right = H_BUTTON_PIN_RIGHT; _servo_pin = H_SERVO_PIN; + _barrier_pin_left = H_LEFT_BARRIER_PIN; + _barrier_pin_right = H_RIGHT_BARRIER_PIN; _us_trigger_pin = H_US_TRIG_PIN; _us_echo_pin = H_US_ECHO_PIN; _drive_left_frwd_bkwrd_pin = H_DRIVE_LEFT_FWRD_BKWRD_PIN; @@ -27,15 +29,15 @@ CodeRacerMKII::CodeRacerMKII() /** @brief CodeRace constructor with pins.This will overwrite the default settings taken from the header file. * @param button_pin Pin the external button is connected at -* @param servo_pin Pin the servo drive is connected at -* @param us_trigger_pin Pin the trigger signal of the ultrasonic sensor is connected at +* @param servo_pin Pin the servo drive is connected at +* @param left_barrier_pin Pin the left photoelectric barrier is connected at +* @param right_barrier_pin Pin the right photoelectric barrier is connected at +* @param us_trigger_pin Pin the trigger signal of the ultrasonic sensor is connected at * @param us_echo_pin Pin the echo signal of the ultrasonic sensor is connected at -* @param drive_left_frwd_pin Pin the forward pin of the left side drive device driver is connected at -* @param drive_left_back_pin Pin the backward pin of the left side drive device driver is connected at -* @param drive_left_enable_pin Pin the enable pin of the left side drive device driver is connected at -* @param drive_right_frwd_pin Pin the forward pin of the right side drive device driver is connected at -* @param drive_right_back_pin Pin the backward pin of the right side drive device driver is connected at -* @param drive_right_enable_pin Pin the enable pin of the right side drive device driver is connected at +* @param drive_left_frwd_bkwrd_pin Pin the forward/ backward pin of the left side drive device driver is connected at +* @param drive_left_enable_pin Pin the enable pin of the left side drive device driver is connected at +* @param drive_right_frwd_bkwrd_pin Pin the forward/ backward pin of the right side drive device driver is connected at +* @param drive_right_enable_pin Pin the enable pin of the right side drive device driver is connected at * @param led_frwd_pin Pin the led that signals forward direction is connected at * @param led_stop_pin Pin the led that signals that the drives are stopped is connected at * @param led_left_pin Pin the led that signals left side direction is connected at @@ -45,6 +47,7 @@ CodeRacerMKII::CodeRacerMKII() * @return nothing */ CodeRacerMKII::CodeRacerMKII(uint8_t button_pin_left , uint8_t button_pin_right, uint8_t servo_pin, + uint8_t left_barrier_pin, uint8_t right_barrier_pin, uint8_t us_trigger_pin, uint8_t us_echo_pin, uint8_t drive_left_frwd_bkwrd_pin, uint8_t drive_left_enable_pin, uint8_t drive_right_frwd_bkwrd_pin, uint8_t drive_right_enable_pin, @@ -54,6 +57,8 @@ CodeRacerMKII::CodeRacerMKII(uint8_t button_pin_left , uint8_t button_pin_right, _button_pin_left = button_pin_left; _button_pin_right = button_pin_right; _servo_pin = servo_pin; + _barrier_pin_left = left_barrier_pin; + _barrier_pin_right = right_barrier_pin; _us_trigger_pin = us_trigger_pin; _us_echo_pin = us_echo_pin; _drive_left_frwd_bkwrd_pin = drive_left_frwd_bkwrd_pin; @@ -94,10 +99,9 @@ void CodeRacerMKII::begin() { _drive_left_speed = H_DRIVE_LEFT_SPEED; _drive_right_speed = H_DRIVE_RIGHT_SPEED; - Serial.printf("1 speed left %d , speed right %d\n", _drive_left_speed, _drive_right_speed); _turn_left_for_ms = H_RACER_TURN_LEFT_FOR_MS; _turn_right_for_ms = H_RACER_TURN_RIGHT_FOR_MS; - + coderracer_activ = false; _coderracer_activ = true; _drive = false; @@ -118,10 +122,16 @@ void CodeRacerMKII::begin() { // Ultrasonic sensor pinMode(_us_trigger_pin, OUTPUT); pinMode(_us_echo_pin, INPUT); - + // Servo drive _servo->attach(_servo_pin); + // PE barriers + pinMode(_barrier_pin_left, INPUT_PULLUP); + attachInterrupt(digitalPinToInterrupt(_barrier_pin_left), _count_steps_left, FALLING); + pinMode(_barrier_pin_right, INPUT_PULLUP); + attachInterrupt(digitalPinToInterrupt(_barrier_pin_right), _count_steps_right, FALLING); + // Left drive pinMode(_drive_left_frwd_bkwrd_pin, OUTPUT); set_drive_left_state(DRIVESTOP); @@ -363,12 +373,10 @@ bool CodeRacerMKII::start_stop() { _coderracer_activ = coderracer_activ; if (true == _coderracer_activ) { set_leds_all_off(); - delay(500); } else { stop_driving(); set_leds_all_off(); - delay(200); servo_set_to_center(); _servo_look_around_at_ms = millis() + random(20000, 120000); } @@ -449,6 +457,12 @@ unsigned int CodeRacerMKII::button_count() { return(right_button_count); } +unsigned int CodeRacerMKII::show_distance_mm(){ + int driven_distance; + driven_distance= left_step_counter*DISTANCE_PER_TICK_MM; + return driven_distance; +} + /** @} */ // end of group higherlevelgetters @@ -1034,6 +1048,17 @@ unsigned long CodeRacerMKII::usonic_distance_cm() { * @{ */ +/** @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 +* @return nothing +*/ +void CodeRacerMKII::speed_settings(uint8_t drive_left_speed, uint8_t drive_right_speed) +{ + _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 @@ -1103,6 +1128,14 @@ void CodeRacerMKII::set_drive_state(drivestate state, uint8_t frwdbackpin, uint8 } } +void CodeRacerMKII::set_syncstop(bool state){ + if (1== state) + { + syncstop=1; + } + else syncstop=0; +} + /** @brief Sets the speed for both of the drives. * * 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 @@ -1174,6 +1207,63 @@ void CodeRacerMKII::_analog_write(uint8_t pin, uint8_t speed) { } } +void CodeRacerMKII::drive_turns(unsigned int turnsleft, unsigned int turnsright){ + drive_ticks(turnsleft*TURN, turnsright*TURN); +} + +void CodeRacerMKII::drive_ticks(unsigned int tickleft, unsigned int tickright){ + drive_ticks_left(tickleft, false); + drive_ticks_right(tickright, false); + drive_forward(); +} + +void CodeRacerMKII::turn_degree(int degree){ + if(degree>0) + { + set_drives_states_left_right(DRIVEFRWD, DRIVEBACK); + } + else if (degree<0) + { + set_drives_states_left_right(DRIVEBACK, DRIVEFRWD); + degree= abs(degree); + } + else set_drives_states_left_right(DRIVESTOP, DRIVESTOP); + + 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); +} + +void CodeRacerMKII::drive_ticks_left(unsigned int Ticks, bool drive = true ){ + if(left_step_stopcount<=left_step_counter) + { + left_step_stopcount = left_step_counter+ Ticks; + if(true == drive){drive_forward();} + } +} + +void CodeRacerMKII::drive_ticks_right(unsigned int Ticks, bool drive = true){ + if(right_step_stopcount<=right_step_counter) + { + right_step_stopcount = right_step_counter+ Ticks; + if(true == drive){drive_forward();} + } +} + +void CodeRacerMKII::drive_distance_mm(unsigned int leftdistance, unsigned int rightdistance){ + drive_distance_left_mm(leftdistance); + drive_distance_right_mm(rightdistance); +} + +void CodeRacerMKII::drive_distance_left_mm(int distance_mm){ + distance_mm/=DISTANCE_PER_TICK_MM; + drive_ticks_left(distance_mm); +} +void CodeRacerMKII::drive_distance_right_mm(int distance_mm){ + distance_mm/=DISTANCE_PER_TICK_MM; + drive_ticks_right(distance_mm); +} /** @} */ // end of group lowerleveldrivesgetters /** @} */ // end of group lowerleveldrives @@ -1256,6 +1346,46 @@ void IRAM_ATTR CodeRacerMKII::_set_button_state_right() { } +void IRAM_ATTR CodeRacerMKII::_count_steps_left() { + left_step_counter++; + if (left_step_counter==left_step_stopcount) + { + //TODO: use stop_driving(); + ledcWrite(DRIVE_PWM_LEFT_CHANNEL, 0); + if(true == syncstop) + { + ledcWrite(DRIVE_PWM_RIGHT_CHANNEL, 0); + } + digitalWrite(H_LED_RIGHT_PIN, LOW); + digitalWrite(H_LED_LEFT_PIN, LOW); + digitalWrite(H_LED_STOP_PIN, HIGH); + digitalWrite(H_LED_FRWD_PIN, LOW); + digitalWrite(H_LED_BLUE_PIN, LOW); + digitalWrite(H_LED_WHITE_PIN, LOW); + } +} + +void IRAM_ATTR CodeRacerMKII::_count_steps_right() { + right_step_counter++; + if (right_step_counter==right_step_stopcount) + { + //TODO: use stop_driving(); + ledcWrite(DRIVE_PWM_RIGHT_CHANNEL, 0); + if(true == syncstop) + { + ledcWrite(DRIVE_PWM_LEFT_CHANNEL, 0); + } + digitalWrite(H_LED_RIGHT_PIN, LOW); + digitalWrite(H_LED_LEFT_PIN, LOW); + digitalWrite(H_LED_STOP_PIN, HIGH); + digitalWrite(H_LED_FRWD_PIN, LOW); + digitalWrite(H_LED_BLUE_PIN, LOW); + digitalWrite(H_LED_WHITE_PIN, LOW); + } +} + + + //********************************************************************************************************************** //** Helper methods //********************************************************************************************************************** diff --git a/vsode/coderacer_mkII/lib/CodeRacer_MKII/CodeRacer_MKII.h b/vsode/coderacer_mkII/lib/CodeRacer_MKII/CodeRacer_MKII.h index c549bd6..381d3e8 100644 --- a/vsode/coderacer_mkII/lib/CodeRacer_MKII/CodeRacer_MKII.h +++ b/vsode/coderacer_mkII/lib/CodeRacer_MKII/CodeRacer_MKII.h @@ -37,6 +37,18 @@ #define SERVO_MIN_POSITION 10 // minimum servo position #define SERVO_SET_1TICK_POSITION_DELAY_MS 3 // minimum duration of time between two servo steps +//----- PE- Barrier ----- +#define H_LEFT_BARRIER_PIN 34 +#define H_RIGHT_BARRIER_PIN 35 +#define TURN 20 +#define WHEEL_DIAMETER_MM 70 +#define SCOPE_OF_WHEEL PI*WHEEL_DIAMETER_MM +#define DISTANCE_PER_TICK_MM SCOPE_OF_WHEEL/TURN +#define DISTANCE_BETWEEN_WHEELS_MM 112 +#define SCOPE_OF_ONE_TURN_MM PI*DISTANCE_BETWEEN_WHEELS_MM +#define TICKS_OF_ONE_TURN SCOPE_OF_ONE_TURN_MM/DISTANCE_PER_TICK_MM +#define TICKS_OF_ONE_DEGREE TICKS_OF_ONE_TURN/360 + //----- Ultrasonic sensor ----- #define H_US_TRIG_PIN 21 #define H_US_ECHO_PIN 17 @@ -67,6 +79,11 @@ using namespace std; +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 coderracer_activ = false; static volatile bool right_button_pressed =false; static volatile unsigned int right_button_count=0; @@ -101,6 +118,8 @@ class CodeRacerMKII { uint8_t _button_pin_left; uint8_t _button_pin_right; uint8_t _servo_pin; + uint8_t _barrier_pin_left; + uint8_t _barrier_pin_right; uint8_t _us_trigger_pin; uint8_t _us_echo_pin; uint8_t _drive_left_frwd_bkwrd_pin; @@ -149,7 +168,8 @@ class CodeRacerMKII { //objects Servo* _servo; Servo* _servo_dummy; - + static void _count_steps_left(); + static void _count_steps_right(); static void _set_button_state_left(); static void _set_button_state_right(); void _analog_write(uint8_t pin, uint8_t speed); @@ -169,6 +189,7 @@ class CodeRacerMKII { CodeRacerMKII(); CodeRacerMKII(uint8_t button_pin_left, uint8_t button_pin_right, uint8_t servo_pin, + uint8_t barrier_pin_left, uint8_t barrier_pin_right, uint8_t us_trigger_pin, uint8_t us_echo_pin, uint8_t drive_left_frwd_bkwrd_pin, uint8_t drive_left_enable_pin, uint8_t drive_right_frwd_bkwrd_pin, uint8_t drive_right_enable_pin, @@ -194,6 +215,7 @@ class CodeRacerMKII { unsigned long turn_right_for_ms(); bool right_button_state(); unsigned int button_count(); + unsigned int show_distance_mm(); // higher level {code}racer services @@ -213,6 +235,8 @@ class CodeRacerMKII { void start_stop_at_min_distance(unsigned long min_distance_cm); void stop_stop_at_min_distance(); bool start_stop(); + + // Bluetooth void bt_enable_stopOnLostConnection(); void bt_enable_stopOnLostConnection(unsigned long timeout); @@ -232,16 +256,26 @@ class CodeRacerMKII { void set_leds_all_on(); // Drives + void speed_settings(uint8_t drive_left_speed, uint8_t drive_right_speed); void drives_settings(uint8_t drive_left_speed, uint8_t drive_right_speed, unsigned long turn_left_ms, unsigned long turn_right_ms); void set_drives_states_left_right(drivestate stateleft, drivestate stateright); void set_drive_left_state(drivestate state); void set_drive_right_state(drivestate state); + void set_syncstop(bool state); void set_drive_state(drivestate state, uint8_t fwrdbackpin, uint8_t enable_pin); void set_drives_speed_left_right(uint8_t speedleft, uint8_t speedright); void set_drive_left_speed(uint8_t speed); void set_drive_right_speed(uint8_t speed); void set_drive_speed(uint8_t speed, uint8_t enablepin); void set_drives_stop_left_right(); + void drive_turns(unsigned int turnsleft, unsigned int turnsright); + void drive_ticks(unsigned int tickleft, unsigned int tickright); + void turn_degree(int degree); + void drive_ticks_left(unsigned int Ticks, bool drive); + void drive_ticks_right(unsigned int Ticks, bool drive); + void drive_distance_mm(unsigned int leftdistance, unsigned int rightdistance); + void drive_distance_left_mm(int distance_mm); + void drive_distance_right_mm(int distance_mm); // Ultrasonic sensor unsigned long usonic_measure_cm(); diff --git a/vsode/coderacer_mkII/src/coderacer_main.cpp b/vsode/coderacer_mkII/src/coderacer_main.cpp index 77067db..820d0fb 100644 --- a/vsode/coderacer_mkII/src/coderacer_main.cpp +++ b/vsode/coderacer_mkII/src/coderacer_main.cpp @@ -31,7 +31,7 @@ void setup() { anzahl_drehung = 0; drehung = links; - + coderacer.set_syncstop(true); } //---- Hier startet unsere endlose Schleife - die immer wieder von vorn angefangen wird, wenn wir am Ende angekommen sind. Da ist unser "Fahr"Code drin, der den CodeRacer steuert @@ -39,26 +39,13 @@ void loop() { while(coderacer.start_stop()== 1) { - coderacer.servo_sweep(); - int iAbstand= coderacer.usonic_measure_cm(); - coderacer.drive_forward(130, 108); - if (iAbstand<12) - { - coderacer.stop_driving(); - delay(1000); - iAbstand= coderacer.usonic_measure_cm(); - - - while (iAbstand<25) - { - coderacer.drive_backward(); - iAbstand= coderacer.usonic_measure_cm(); - } - coderacer.stop_driving(); - delay(1000); - coderacer.turn_left(400); - delay(1000); - } + delay(2000); + coderacer.speed_settings(130, 130); + coderacer.turn_degree(180); + while(true== coderacer.is_driving()) + {}; + coderacer.turn_degree(-180); + delay(5000); } }