New MKII update with implementation of step- counter and some tweaks.
This commit is contained in:
parent
c889e6a8ec
commit
e0db522d10
3 changed files with 186 additions and 35 deletions
|
@ -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,7 +99,6 @@ 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;
|
||||
|
||||
|
@ -122,6 +126,12 @@ void CodeRacerMKII::begin() {
|
|||
// 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
|
||||
//**********************************************************************************************************************
|
||||
|
|
|
@ -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();
|
||||
|
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
Loading…
Reference in a new issue