82 lines
2.1 KiB
C++
82 lines
2.1 KiB
C++
#include "CodeRacer_MKII.h"
|
|
|
|
//----- settings for the ultrasonic sensor -----
|
|
#define US_STOP_ABSTAND_CM 20 // if distance goes below that - stop the racer
|
|
|
|
//----- variables we need
|
|
unsigned long distance_cm = 0;
|
|
|
|
//---- construct the coderacer object
|
|
CodeRacerMKII coderacer;
|
|
|
|
//---- set up code - executed ones
|
|
void setup1() {
|
|
// start serial monitor
|
|
Serial.begin(115200);
|
|
// initialize the coderacer
|
|
coderacer.begin();
|
|
// enable fun stuff
|
|
coderacer.coderacer_fun_enabled = true;
|
|
// look to the left, to the right and to center... :-)
|
|
coderacer.servo_set_to_left();
|
|
delay(100);
|
|
coderacer.servo_set_to_right();
|
|
delay(100);
|
|
coderacer.servo_set_to_center();
|
|
delay(100);
|
|
}
|
|
|
|
//---- 'endless' loop
|
|
void loop1() {
|
|
|
|
// check if the racer was started (button was toggled to coderacer active state
|
|
if(true == coderacer.start_stop()){
|
|
|
|
Serial.print("Speed of right side drive: ");
|
|
Serial.println(coderacer.drive_right_speed());
|
|
Serial.print("Speed of left side drive: ");
|
|
Serial.println(coderacer.drive_left_speed());
|
|
|
|
// measure the distance - at the position of the servo
|
|
distance_cm = coderacer.usonic_measure_cm();
|
|
|
|
coderacer.start_stop_at_min_distance(US_STOP_ABSTAND_CM);
|
|
while(!coderacer.stopped_at_min_distance()){
|
|
|
|
Serial.print("Distanc in cm: ");
|
|
Serial.println(distance_cm);
|
|
|
|
if(distance_cm > 50){
|
|
coderacer.drive_forward();
|
|
coderacer.servo_sweep();
|
|
}
|
|
else if(distance_cm > 40){
|
|
coderacer.turn_right();
|
|
}
|
|
else if(distance_cm > 30){
|
|
coderacer.turn_left();
|
|
}
|
|
else {
|
|
|
|
coderacer.drive_backward();
|
|
}
|
|
|
|
// measure the distance - at the position of the servo
|
|
distance_cm = coderacer.usonic_measure_cm();
|
|
|
|
}
|
|
|
|
Serial.println("***** STOPPED ***** ");
|
|
Serial.print("Measured stop distanc of cm: ");
|
|
Serial.println(distance_cm);
|
|
Serial.print("Measured at servo position of: ");
|
|
Serial.println(coderacer.servo_position());
|
|
|
|
coderacer.set_inactive();
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|