Commit 462dbb3d authored by Jonathan Glück's avatar Jonathan Glück

New MKII version of library

parent d5754d0a
......@@ -18,10 +18,11 @@
#define FUN_MAX_PAUSE_MS 300000
#define LED_SWITCH_MS 50 // speed of knight rider lights
//----- Button ------------
#define H_BUTTON_PIN 17
#define H_BUTTON_PIN_LEFT 13
#define H_BUTTON_PIN_RIGHT 16
#define BUTTON_BOUNCING_TIME_MS 200 // bouncing delay
//----- Servo -----
#define H_SERVO_PIN 16
#define H_SERVO_PIN 18
#define H_SERVO_LEFT_POS 145 // left position of the servo
#define H_SERVO_CENTER_LEFT 100 // left-center position of the servo
#define H_SERVO_RIGHT_POS 35 // right position of the servo
......@@ -37,20 +38,18 @@
#define SERVO_SET_1TICK_POSITION_DELAY_MS 3 // minimum duration of time between two servo steps
//----- Ultrasonic sensor -----
#define H_US_TRIG_PIN 12
#define H_US_ECHO_PIN 14
#define H_US_TRIG_PIN 21
#define H_US_ECHO_PIN 17
#define H_US_STOP_DISTANCE_CM 25 // if the measured distance is smaller the racer maybe stopped
#define US_MAX_ECHO_TIME_US 6000 // timeout for ultrasonic sensor measurements - this is about 100cm
//----- Drives -----
#define H_DRIVE_RIGHT_SPEED 255 // default speed of right side drive. 0 ...255
#define H_DRIVE_LEFT_SPEED 255 // default speed of left side drive. 0 ...255
#define H_DRIVE_RIGHT_ENABLE_PIN 2
#define H_DRIVE_RIGHT_FWRD_PIN 4
#define H_DRIVE_RIGHT_BACK_PIN 15
#define H_DRIVE_LEFT_ENABLE_PIN 21
#define H_DRIVE_LEFT_FWRD_PIN 22
#define H_DRIVE_LEFT_BACK_PIN 23
#define H_DRIVE_RIGHT_SPEED 128 // default speed of right side drive. 0 ...255
#define H_DRIVE_LEFT_SPEED 128 // default speed of left side drive. 0 ...255
#define H_DRIVE_RIGHT_ENABLE_PIN 5
#define H_DRIVE_RIGHT_FWRD_BKWRD_PIN 19
#define H_DRIVE_LEFT_ENABLE_PIN 23
#define H_DRIVE_LEFT_FWRD_BKWRD_PIN 22
#define H_RACER_TURN_LEFT_FOR_MS 400 // duration of time the racer will turn to left
#define H_RACER_TURN_RIGHT_FOR_MS 400 // duration of time the racer will turn to right
......@@ -58,16 +57,21 @@
#define DRIVE_PWM_RIGHT_CHANNEL 6 // PWM-channel for right side drive
//----- LEDs -----
#define H_LED_FRWD_PIN 26
#define H_LED_FRWD_PIN 27
#define H_LED_STOP_PIN 25
#define H_LED_LEFT_PIN 33
#define H_LED_RIGHT_PIN 27
#define H_LED_LEFT_PIN 32
#define H_LED_RIGHT_PIN 33
#define H_LED_BLUE_PIN 14
#define H_LED_WHITE_PIN 12
using namespace std;
static volatile bool coderracer_activ = false;;
static volatile unsigned long button_last_pressed_at_ms = millis();
static volatile bool coderracer_activ = false;
static volatile bool right_button_pressed =false;
static volatile unsigned int right_button_count=0;
static volatile unsigned long left_button_last_pressed_at_ms = millis();
static volatile unsigned long right_button_last_pressed_at_ms = millis();
enum ledstate {
LEDOFF,
......@@ -94,20 +98,21 @@ class CodeRacerMKII {
BluetoothSerial* _BTSerial;
//pins
uint8_t _button_pin;
uint8_t _button_pin_left;
uint8_t _button_pin_right;
uint8_t _servo_pin;
uint8_t _us_trigger_pin;
uint8_t _us_echo_pin;
uint8_t _drive_left_frwd_pin;
uint8_t _drive_left_back_pin;
uint8_t _drive_left_frwd_bkwrd_pin;
uint8_t _drive_left_enable_pin;
uint8_t _drive_right_frwd_pin;
uint8_t _drive_right_back_pin;
uint8_t _drive_right_frwd_bkwrd_pin;
uint8_t _drive_right_enable_pin;
uint8_t _led_frwd_pin;
uint8_t _led_stop_pin;
uint8_t _led_left_pin;
uint8_t _led_right_pin;
uint8_t _led_blue_pin;
uint8_t _led_white_pin;
//servo variables
int8_t _servo_sweep_step;
......@@ -135,7 +140,6 @@ class CodeRacerMKII {
uint8_t _last_led_on;
unsigned long _servo_look_around_at_ms;
unsigned long _min_distance_cm;
bool _drive;
unsigned long _drive_set_at_ms;
......@@ -146,7 +150,8 @@ class CodeRacerMKII {
Servo* _servo;
Servo* _servo_dummy;
static void _set_button_state();
static void _set_button_state_left();
static void _set_button_state_right();
void _analog_write(uint8_t pin, uint8_t speed);
unsigned long _servo_set_position(uint8_t position);
......@@ -163,11 +168,11 @@ class CodeRacerMKII {
//methods
CodeRacerMKII();
CodeRacerMKII(uint8_t button_pin, uint8_t servo_pin,
CodeRacerMKII(uint8_t button_pin_left, uint8_t button_pin_right, uint8_t servo_pin,
uint8_t us_trigger_pin, uint8_t us_echo_pin,
uint8_t drive_left_frwd_pin, uint8_t drive_left_back_pin, uint8_t drive_left_enable_pin,
uint8_t drive_right_frwd_pin, uint8_t drive_right_back_pin, uint8_t drive_right_enable_pin,
uint8_t led_frwd_pin, uint8_t led_stop_pin, uint8_t led_left_pin, uint8_t led_right_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,
uint8_t led_frwd_pin, uint8_t led_stop_pin, uint8_t led_left_pin, uint8_t led_right_pin, uint8_t led_blue_pin, uint8_t led_white_pin);
void set_inactive();
void set_active();
......@@ -187,6 +192,9 @@ class CodeRacerMKII {
uint8_t drive_right_speed();
unsigned long turn_left_for_ms();
unsigned long turn_right_for_ms();
bool right_button_state();
unsigned int button_count();
// higher level {code}racer services
void stop_driving();
......@@ -205,7 +213,6 @@ 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);
......@@ -219,7 +226,7 @@ class CodeRacerMKII {
void bt_removeStringFromIgnoreList(String stringtoignore);
// LEDs
void set_leds_left_stop_frwd_right(ledstate leftled, ledstate stopled, ledstate frwdled, ledstate rightled);
void set_leds_left_stop_frwd_right(ledstate leftled, ledstate stopled, ledstate frwdled, ledstate rightled, ledstate blueled, ledstate whiteled);
void set_leds_all(ledstate alleds);
void set_leds_all_off();
void set_leds_all_on();
......@@ -229,7 +236,7 @@ class CodeRacerMKII {
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_drive_state(drivestate state, uint8_t frwdpin, uint8_t backpin);
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);
......
......@@ -18,7 +18,7 @@ CodeRacerMKII coderacer;
//---- Hier startet der Code zum Einstellen aller wichtigen Dinge. Setup() wird einmal ausgeführt. ----
void setup() {
// Monitor
Serial.begin(115200); // Serial Monitor aktivieren. Mit dem Monitor kann man sich Werte und Meldungen anzeigen lassen.
Serial.begin(9600); // Serial Monitor aktivieren. Mit dem Monitor kann man sich Werte und Meldungen anzeigen lassen.
// CodeRacer initialisieren
coderacer.begin();
......@@ -37,92 +37,29 @@ void setup() {
//---- 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
void loop() {
// Abstand messen -> nach vorn ... um zu sehen, ob was passiert messen wir immer ... auch wenn der Racer nicht fahren soll
abstand_vorn_cm = coderacer.usonic_measure_cm();
// Abfragen ob der Racer fahren soll oder nicht ...
if(true == coderacer.start_stop()){
// Abstandssensor schon verstellen ... dann hat er das bis zur nächsten Messung auch geschafft
while(coderacer.start_stop()== 1)
{
coderacer.servo_sweep();
// Ist die Bahn frei?
if(abstand_vorn_cm < US_STOP_ABSTAND_CM){
// Nein! Der Abstand nach vorn ist kleiner als erlaubt!
// Racer anhalten
int iAbstand= coderacer.usonic_measure_cm();
coderacer.drive_forward(130, 108);
if (iAbstand<12)
{
coderacer.stop_driving();
// Nach links schauen!
coderacer.servo_set_to_left();
// Abstand messen und merken.
abstand_links_cm = coderacer.usonic_measure_cm();
// Nach rechts schauen!
coderacer.servo_set_to_right();
// Abstand messen und merken.
abstand_rechts_cm = coderacer.usonic_measure_cm();
delay(1000);
iAbstand= coderacer.usonic_measure_cm();
// Ist der Abstand links oder rechts groß genug genug?
// Wenn nicht - wenn der Racer sich bisher noch nicht gedreht hat - fahre ein Stück rückwarts. Wenn der Racer sich gerade schon gedreht hat, drehe in dieselbe Richtung wie vorher.
if((abstand_links_cm < US_MIN_ABSTAND_LI_RE) && (abstand_rechts_cm < US_MIN_ABSTAND_LI_RE)){
if(anzahl_drehung == 0){
coderacer.drive_backward();
delay(300);
coderacer.stop_driving();
//noch mal Abstand messen ...
coderacer.servo_set_to_left();
// Abstand messen und merken.
abstand_links_cm = coderacer.usonic_measure_cm();
// Nach rechts schauen!
coderacer.servo_set_to_right();
// Abstand messen und merken.
abstand_rechts_cm = coderacer.usonic_measure_cm();
}
if( anzahl_drehung > MAX_ANZAHL_DREHUNGEN ){
anzahl_drehung = 0;
} else {
anzahl_drehung ++;
}
}
// wenn der Racer sich noch nicht gedreht hat (anzahl_drehung == 0) - oder gerade rückwärts gefahren ist (anzahl_drehung == 1) drehe jetzt dahin wo mehr Platz ist;
if(anzahl_drehung <= 1){
// Welcher Abstand ist größer?
if(abstand_links_cm > abstand_rechts_cm){
// Links ist mehr Platz!
drehung = links;
}
else{
// Rechts ist mehr Platz!
drehung = rechts;
}
}
switch(drehung){
case links:
coderacer.turn_left();
coderacer.servo_set_to_center();
break;
case rechts:
coderacer.turn_right();
coderacer.servo_set_to_center();
break;
while (iAbstand<20)
{
coderacer.drive_backward();
iAbstand= coderacer.usonic_measure_cm();
}
coderacer.stop_driving();
delay(1000);
coderacer.turn_left(300);
delay(1000);
}
else{
// Ja! Die Bahn ist frei
// Wenn die Bahn richtig frei ist, dann können wir den Zähler fürs drehen auf 0 setzen ...
if( abstand_vorn_cm > (US_STOP_ABSTAND_CM + US_MIN_ABSTAND_LI_RE)){
anzahl_drehung = 0;
}
coderacer.drive_forward();
}
} else {
anzahl_drehung = 0;
}
}
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