Commit e0db522d authored by Jonathan Glück's avatar Jonathan Glück

New MKII update with implementation of step- counter and some tweaks.

parent c889e6a8
Pipeline #77 canceled with stage
......@@ -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
//**********************************************************************************************************************
......
......@@ -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);
}
}
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment