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

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;
@@ -28,13 +30,13 @@ 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	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_frwd_bkwrd_pin			Pin the forward/ backward 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_frwd_bkwrd_pin			Pin the forward/ backward 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	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
@@ -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
 //**********************************************************************************************************************

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();

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();
+    delay(2000);
-    int iAbstand= coderacer.usonic_measure_cm();
+    coderacer.speed_settings(130, 130);
-    coderacer.drive_forward(130, 108);
+    coderacer.turn_degree(180);
-    if (iAbstand<12)
+    while(true== coderacer.is_driving())
-    {
+    {};
-      coderacer.stop_driving();
+    coderacer.turn_degree(-180);
-      delay(1000);
+    delay(5000);
-      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);
-    }
   }
 }