Skip to content

Commit

Permalink
line following and collision avoidance
Browse files Browse the repository at this point in the history
  • Loading branch information
@LeeHounshell
LeeHounshell committed Oct 9, 2019
1 parent b5fe71d commit e2354b0
Showing 1 changed file with 131 additions and 64 deletions.
195 changes: 131 additions & 64 deletions robot/batbot.sketch
View file
Original file line number Diff line number Diff line change
Expand Up @@ -78,6 +78,7 @@

int Echo = A4; // ultrasonic
int Trig = A5; // ultrasonic
int boost = 0; // power boost

//----------------------------------------
//--------------- TIMING -----------------
Expand Down Expand Up @@ -105,7 +106,8 @@ unsigned int previous_lt_r, previous_lt_m, previous_lt_l;
//----------------------------------------
//--------------- CAR SPEED --------------

#define DEFAULT_SPEED 200 // initial speed of car >=0 to <=255
#define TURN_SPEED_ADJ 75 // extra speed for turning
#define DEFAULT_SPEED 150 // initial speed of car >=0 to <=255
#define MAX_CAR_SPEED 255 // max speed of car
#define OFFSET_SPEED (MAX_CAR_SPEED - DEFAULT_SPEED)

Expand Down Expand Up @@ -214,43 +216,83 @@ int speed_delta() {
}

void forward(){
if (*motion != 'f') {
boost = 30;
}
else {
boost = boost / 2;
}
motion = "forward";
direction = UPARROW;
analogWrite(ENA, DEFAULT_SPEED + speed_delta());
analogWrite(ENB, DEFAULT_SPEED + speed_delta());
int speed = DEFAULT_SPEED + speed_delta() + boost;
if (speed > 255) {
speed = 255;
}
analogWrite(ENA, speed);
analogWrite(ENB, speed);
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
}

void back() {
if (*motion != 'b') {
boost = 30;
}
else {
boost = boost / 2;
}
motion = "back";
direction = DOWNARROW;
analogWrite(ENA, DEFAULT_SPEED + speed_delta());
analogWrite(ENB, DEFAULT_SPEED + speed_delta());
int speed = DEFAULT_SPEED + speed_delta() + boost;
if (speed > 255) {
speed = 255;
}
analogWrite(ENA, speed);
analogWrite(ENB, speed);
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
digitalWrite(IN3, HIGH);
digitalWrite(IN4, LOW);
}

void left() {
if (*motion != 'l') {
boost = 30;
}
else {
boost = boost / 2;
}
motion = "left";
direction = LEFTARROW;
analogWrite(ENA, DEFAULT_SPEED + speed_delta());
analogWrite(ENB, DEFAULT_SPEED + speed_delta());
int speed = DEFAULT_SPEED + speed_delta() + TURN_SPEED_ADJ + boost;
if (speed > 255) {
speed = 255;
}
analogWrite(ENA, speed);
analogWrite(ENB, speed);
digitalWrite(IN1, LOW);
digitalWrite(IN2, HIGH);
digitalWrite(IN3, LOW);
digitalWrite(IN4, HIGH);
}

void right() {
if (*motion != 'r') {
boost = 30;
}
else {
boost = boost / 2;
}
motion = "right";
direction = RIGHTARROW;
analogWrite(ENA, DEFAULT_SPEED + speed_delta());
analogWrite(ENB, DEFAULT_SPEED + speed_delta());
int speed = DEFAULT_SPEED + speed_delta() + TURN_SPEED_ADJ + boost;
if (speed > 255) {
speed = 255;
}
analogWrite(ENA, speed);
analogWrite(ENB, speed);
digitalWrite(IN1, HIGH);
digitalWrite(IN2, LOW);
digitalWrite(IN3, HIGH);
Expand Down Expand Up @@ -836,7 +878,7 @@ boolean function_left_spin() {
println("$ spin left", 1);
left();
cmdMillis = millis();
cmdMillisLoop = 9;
cmdMillisLoop = 5;
have_cmd = 1;
return 1;
}
Expand All @@ -845,6 +887,7 @@ boolean function_left() {
println("$ go left", 1);
left();
cmdMillis = millis();
cmdMillisLoop = 2;
have_cmd = 1;
return 1;
}
Expand All @@ -853,7 +896,7 @@ boolean function_right_spin() {
println("$ spin right", 1);
right();
cmdMillis = millis();
cmdMillisLoop = 9;
cmdMillisLoop = 5;
have_cmd = 1;
return 1;
}
Expand All @@ -862,6 +905,7 @@ boolean function_right() {
println("$ go right", 1);
right();
cmdMillis = millis();
cmdMillisLoop = 2;
have_cmd = 1;
return 1;
}
Expand Down Expand Up @@ -909,6 +953,19 @@ boolean function_four() {
return 1;
}

//
// TODO: communicate with the Nano to map everything intelligently
//
// map your world
boolean function_five() {
if (*state != 'm') {
state = "map";
servoDelta = 10;
println("$ 'map'", 1);
}
return 1;
}

// look full left
boolean function_six() {
println("$ look full right", 1);
Expand Down Expand Up @@ -966,19 +1023,6 @@ boolean function_nine() {
return 1;
}

//
// TODO: communicate with the Nano to map everything intelligently
//
// map your world
boolean function_five() {
if (*state != 'm') {
state = "map";
servoDelta = 10;
println("$ 'map'", 1);
}
return 1;
}

// run security monitor protocol
boolean function_zero() {
if (*state != 's') {
Expand All @@ -989,101 +1033,125 @@ boolean function_zero() {
return 1;
}

// run Elegoo collision avoidance modified for Serial interrupt
// run collision avoidance mode
boolean function_star() {
if ((millis() - cmdMillis) < 1500) {
return 1; // wait for a second
}
if (*state != 'a') {
state = "avoid";
println("$ 'avoid'", 1);
}
set_servo_angle(90);
delay(500);
aheadDistance = get_distance();
if (aheadDistance >= 20) {
++avoidState;
if (avoidState > 10) {
avoidState = 1;
}
switch (avoidState) {
case 1: {
set_servo_angle(90);
stop();
break;
}
case 2: {
delay(1000);
aheadDistance = get_distance();
set_servo_angle(10);
break;
}
case 3: {
delay(1000);
rightDistance = get_distance();
set_servo_angle(45);
break;
}
case 4: {
delay(1000);
if (get_distance() < rightDistance) {
rightDistance = get_distance();
}
set_servo_angle(90);
break;
}
case 5: {
delay(1000);
aheadDistance = get_distance();
set_servo_angle(160);
break;
}
case 6: {
delay(1000);
leftDistance = get_distance();
set_servo_angle(135);
break;
}
case 7: {
delay(1000);
if (get_distance() < leftDistance) {
leftDistance = get_distance();
}
set_servo_angle(90);
break;
}
case 8: {
delay(1000);
if (rightDistance > leftDistance) {
function_right();
if (Serial.available()) {
stop();
return 1;
}
aheadDistance = get_distance();
if (aheadDistance >= 30) {
forward();
aheadDistance = get_distance();
}
else if (rightDistance < leftDistance) {
function_left();
if (Serial.available()) {
stop();
return 1;
else {
avoidState = 9;
}
break;
}
case 9: {
avoidState = 0;
aheadDistance = get_distance();
if ((rightDistance <= 25) && (leftDistance <= 25)) {
back();
avoidState = 9; // will be 10 next iteration
}
else if ((rightDistance <= 30) || (leftDistance <= 30)) {
if (rightDistance > leftDistance) {
function_right();
}
else {
function_left();
}
}
else if ((rightDistance <= 20) || (leftDistance <= 20)) {
back(); // TODO: spin instead of backup
cmdMillis = millis();
cmdMillisLoop = 3;
have_cmd = 1;
if (Serial.available()) {
stop();
return 1;
else {
if (aheadDistance >= 30) {
forward();
aheadDistance = get_distance();
}
}
break;
}
case 10: {
avoidState = 0;
if (rightDistance > leftDistance) {
function_right();
}
else {
forward();
cmdMillis = millis();
cmdMillisLoop = 3;
have_cmd = 1;
function_left();
}
break;
}
}
}
else {
back(); // TODO: spin instead of backup
cmdMillis = millis();
cmdMillisLoop = 3;
have_cmd = 1;
back();
avoidState = 9; // will be 10 next iteration
}
if (Serial.available()) {
stop();
avoidState = 0;
}
else {
cmdMillis = millis();
have_cmd = 1;
}
return 1;
}

//
// TODO: stop() case must talk with Jetson Nano for recovery instructions
//
// run Elegoo line following modified for Serial interrupt
// run line following mode
boolean function_sharp() {
if (*state != 'f') {
state = "following";
Expand All @@ -1096,7 +1164,6 @@ boolean function_sharp() {
if (LT_M) {
forward();
cmdMillis = millis();
cmdMillisLoop = 3;
have_cmd = 1;
}
else if (! LT_L && ! LT_M && ! LT_R) {
Expand Down

0 comments on commit e2354b0

Please sign in to comment.