Skip to content

Paxton line follow - do not merge - #17

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
paxtonsc wants to merge 10 commits into
base: main
Choose a base branch
from
Open

Paxton line follow - do not merge - #17

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
333aef2
begining of simple line follwing code
Mar 2, 2022
580fd94
code can now follow straight line, not turns yet
Mar 2, 2022
231e139
code should hopefully get us to the place were we turn 90 degrees
paxtonsc Mar 2, 2022
9943385
adding a couple comments
paxtonsc Mar 2, 2022
d00557d
robot moving from one side of the course to the other
paxtonsc Mar 2, 2022
32b20cd
time change
paxtonsc Mar 2, 2022
c622d4e
adding complexity to line following. Next step clean up code
paxtonsc Mar 3, 2022
2916429
couple small cleanup things
paxtonsc Mar 3, 2022
e4cd6cb
line follow loading zone update
Mar 3, 2022
2d7cdf6
small line follow ABC update
Mar 3, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion Kabob_Code/src/Sensors/LineSensor.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,7 @@ class LineSensor
int read();
int readAnalog(); //adding for debug purposes
int getPin();
const int lineThreshold = 600;
const int lineThreshold = 400;
private:
int pin;
};
261 changes: 233 additions & 28 deletions Kabob_Code/src/StateMachine/StateMachine.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,29 +1,45 @@
#include "StateMachine.h"

#define VELOCITY 200

States_t state;
States_r line_state;
Zones_t zone;

unsigned long state_time;
unsigned long serial_time;
uint8_t servoPin = 6;
uint8_t servoPos = 0;
unsigned long curr_time;

@polygnomial polygnomial Mar 3, 2022

Copy link
Copy Markdown
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

nice, these names are more clear now

@polygnomial polygnomial Mar 3, 2022

Copy link
Copy Markdown
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

or wait you just added more, damnnnn thats a lot lol

unsigned long zone_time;

uint8_t flagLeftLine = 0;
uint8_t flagRightLine = 0;

uint8_t DEBUG = false;

unsigned long flag_time;

void setup() {
// put your setup code here, to run once:
Serial.begin(9600);
while(!Serial);
//delay(10000);

//timer
//times
curr_time = millis();
serial_time = millis();
state_time = millis();
flag_time = millis();

state = STATE_IDLE;
changeStateTo(STATE_LOAD);

Serial.println("Setup Complete!");
}

void loop() {
curr_time = millis();
checkGlobalEvents();
shephard.activity();
checkFlags();
checkForZoneChange();

switch(state) {
case STATE_IDLE:
Expand All @@ -36,61 +52,244 @@ void loop() {
handleNavTargetState();
break;
case STATE_UNLOAD:
shephard.chassis.move_backward_at_speed(100);
handleUnloadState();
break;
case STATE_NAV_LOAD:
handleNavLoadState();
break;
default:
Serial.println("What is this I do not even...");
}

unsigned long current_millis = millis();
if (current_millis - serial_time > MILLISECONDS(1/PRINT_FREQUENCY) ){
Serial.println("One Secound update: ");
Serial.println("left " + String(shephard.sensors.line.left.readAnalog()));
Serial.println("right " + String(shephard.sensors.line.right.readAnalog()));
Serial.println("center left " + String(shephard.sensors.line.center_left.readAnalog()));
Serial.println("center middle " + String(shephard.sensors.line.center_middle.readAnalog()));
Serial.println("center right " + String(shephard.sensors.line.center_right.readAnalog()));
serial_time = current_millis;
//debug for line sensors
if (DEBUG && curr_time - serial_time > MILLISECONDS(1/PRINT_FREQUENCY) ){
Serial.println("---------------");
Serial.println("left " + String(shephard.sensors.line.left.read()));
Serial.println("right " + String(shephard.sensors.line.right.read()));
Serial.println("center left " + String(shephard.sensors.line.center_left.read()));
Serial.println("center middle " + String(shephard.sensors.line.center_middle.read()));
Serial.println("center right " + String(shephard.sensors.line.center_right.read()));
Serial.println("---------------");
serial_time = curr_time;
}

}

void handleLoadState(void) {
int timeInState = millis() - state_time;
if (timeInState < 1500) {
unsigned int timeInState = curr_time - state_time;
if (timeInState < 1000) {
shephard.claw.open();
} else if (timeInState > 3000 && timeInState < 5000) {
} else if (timeInState > 1000 && timeInState < 2000) {
shephard.claw.close();
} else if (timeInState > 5000) {
} else if (timeInState > 2000 && timeInState < 3000) {
// THIS IS WHERE THE ROBOT GETS HOSED
// cannot go straight for more than 1 or 2 secounds
shephard.chassis.move_forward_at_speed(VELOCITY);
} else if (timeInState > 3000) {
changeStateTo(STATE_NAV_TARGET);
changeZoneTo(ZONE_A);
}
}

void handleUnloadState(void) {
int timeInState = curr_time - state_time;
if (timeInState < 1000) {
shephard.claw.open();
}

// back up after depositing ball
else if (timeInState > 1000 && timeInState < 4000) {
shephard.chassis.move_backward_at_speed(VELOCITY);
}
// about 6 secounds to rotate 180 degrees
else if (timeInState > 4000 && timeInState < 10000) {
shephard.chassis.turn_right_at_speed(255);
}

else if (timeInState > 9000) {
changeStateTo(STATE_NAV_LOAD);
}
}

void handleNavTargetState(void){
shephard.chassis.move_forward_at_speed(200);
if (millis() - state_time> 5000) {
changeStateTo(STATE_IDLE);
}
unsigned long t = curr_time - zone_time;
switch(zone) {
case ZONE_LOAD:
break;
case ZONE_A:
shephard.chassis.move_forward_at_speed(200);
break;
case ZONE_B:
if (t < 3000) {
shephard.chassis.turn_left(200);
} else {
lineFollow();
}
break;
case ZONE_C:
if (t < 3000) {
shephard.chassis.turn_left(200);
} else {
lineFollow();
}
break;
case ZONE_1:
if (t < 4000) {
shephard.chassis.move_forward_at_speed(200);
} else {
lineFollow();
}
break;
case ZONE_2:
lineFollow();
break;
case ZONE_3:
// turn 90 degrees and then line follow
if (t > 1000 && t < 5000) {
shephard.chassis.turn_right(200);
} else {
lineFollow();
}
break;
case ZONE_4:

@polygnomial polygnomial Mar 3, 2022

Copy link
Copy Markdown
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Does it behave better with 90° vs like 75° (IIRC the second turn is not a right angle)

@paxtonsc paxtonsc Mar 3, 2022

Copy link
Copy Markdown
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Right now both those turns are hardcoded.

@paxtonsc paxtonsc Mar 3, 2022

Copy link
Copy Markdown
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

In that, we rotate for a certain duration of time.

// turn 90 degrees and then line follow
if (t < 3000) {
shephard.chassis.turn_left(200);
} else {
lineFollow();
}
break;
case ZONE_TARGET:
if (t < 1000) {
shephard.chassis.move_forward_at_speed(VELOCITY);
} else {
changeStateTo(STATE_UNLOAD);
}
break;
default:
Serial.println("Zone has more states than Paxton thought");
}
}

void changeStateTo(States_t s) {
state = s;
state_time = millis();
Serial.println("New state = " + s);
void handleNavLoadState(void) {
shephard.chassis.stop();
}

void checkGlobalEvents(void) {
if (TestForKey()) RespToKey();
}

void checkFlags(void) {
if (curr_time - flag_time > FLAG_TIME) {
flagLeftLine = 0;
flagRightLine = 0;
}
}

void checkForZoneChange(void) {
uint8_t left = shephard.sensors.line.left.read();
uint8_t right = shephard.sensors.line.right.read();
uint8_t center_left = shephard.sensors.line.center_left.read();
uint8_t center_middle = shephard.sensors.line.center_middle.read();
uint8_t center_right = shephard.sensors.line.center_right.read();
// if (left) Serial.println("left sensor tripped");
Serial.println(zone);
if (left && (zone == ZONE_A)) {
changeZoneTo(ZONE_B);
} else if (left && zone == ZONE_B && curr_time - zone_time > 3000) {
changeZoneTo(ZONE_C);
} else if (right && zone == ZONE_C && curr_time - zone_time > 5000) {
changeZoneTo(ZONE_1);
} else if (left && zone == ZONE_1) {
setFlag(flagLeftLine);
changeZoneTo(ZONE_2);
} else if (right && zone == ZONE_2) {
setFlag(flagRightLine);
changeZoneTo(ZONE_3);
} else if (left && zone == ZONE_3 && curr_time - zone_time > 5000) {

@polygnomial polygnomial Mar 3, 2022

Copy link
Copy Markdown
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

why is there both a check that the left sensor is on the line and that time has passed?

@paxtonsc paxtonsc Mar 3, 2022

Copy link
Copy Markdown
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

the time requirement is there because otherwise the state change would be triggered prematurely when the left sensor passes over the black line during the 90 degree turn.

@polygnomial polygnomial Mar 3, 2022

Copy link
Copy Markdown
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

ah copy -- I think there is a way to block the left line sensor check during the turn routine, but if it works it works

changeZoneTo(ZONE_4);
setFlag(flagLeftLine);
} else if (left && zone == ZONE_4 && curr_time - zone_time > 6000) {
changeZoneTo(ZONE_TARGET);
}
}


void lineFollow(void) {
uint8_t center_left = shephard.sensors.line.center_left.read();
uint8_t center_middle = shephard.sensors.line.center_middle.read();
uint8_t center_right = shephard.sensors.line.center_right.read();
// primary line sensing states
if (center_middle && !center_right && !center_left){
changeLineStateTo(STATE_ON_LINE);
}
else if (!center_middle && center_right && !center_left) {
changeLineStateTo(STATE_OFF_LEFT);
}
else if (!center_middle && !center_right && center_left) {
changeLineStateTo(STATE_OFF_RIGHT);
}
else if (center_middle && !center_right && center_left && !flagLeftLine) {
changeLineStateTo(STATE_OFF_SLIGHT_RIGHT);
}
else if (center_middle && center_right && !center_left && !flagRightLine) {
changeLineStateTo(STATE_OFF_SLIGHT_LEFT);
}

switch(line_state) {
case STATE_ON_LINE:
shephard.chassis.move_forward_at_speed(250);
break;
case STATE_OFF_RIGHT:
shephard.chassis.veer_forward(250, 180);
break;
case STATE_OFF_SLIGHT_RIGHT:
shephard.chassis.veer_forward(250, 200);
break;
case STATE_OFF_LEFT:
shephard.chassis.veer_forward(180, 250);
break;
case STATE_OFF_SLIGHT_LEFT:
shephard.chassis.veer_forward(200, 250);
break;
default:
Serial.println("humm");
}

}

uint8_t TestForKey(void) {
uint8_t KeyEventOccurred;
KeyEventOccurred = Serial.available();
return KeyEventOccurred;
}


void changeStateTo(States_t s) {

@polygnomial polygnomial Mar 3, 2022

Copy link
Copy Markdown
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

But in reference to other comments about classes, there are a lot of similarities between these 3 methods

@paxtonsc paxtonsc Mar 3, 2022

Copy link
Copy Markdown
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes.

if (s != state) {
state = s;
state_time = curr_time;
}
}

void changeLineStateTo(States_r s) {
if (s != line_state) {
line_state = s;
}
}

void changeZoneTo(Zones_t z) {
if (z != zone) {
zone = z;
zone_time = millis();
Serial.println("In new zone " + z);
}
}

void setFlag(uint8_t flag){
flag = true;
flag_time = curr_time;
}

void RespToKey(void) {
uint8_t theKey;
theKey = Serial.read();
Expand All @@ -102,7 +301,13 @@ void RespToKey(void) {
case ' ':
state = STATE_IDLE;
break;
case 'r':
changeStateTo(STATE_NAV_TARGET);
break;
default:
break;
}
}



23 changes: 22 additions & 1 deletion Kabob_Code/src/StateMachine/StateMachine.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,24 +6,45 @@

#define PRINT_FREQUENCY 1 // hz
#define MILLISECONDS(time)({time * 1000;})
#define FLAG_TIME 1000

/*---------------State Definitions--------------------------*/
typedef enum {
STATE_IDLE, STATE_LOAD, STATE_NAV_TARGET, STATE_UNLOAD, STATE_NAV_LOAD
STATE_IDLE, STATE_LOAD, STATE_NAV_TARGET, STATE_UNLOAD,
STATE_NAV_LOAD,
} States_t;

typedef enum {

@polygnomial polygnomial Mar 3, 2022

Copy link
Copy Markdown
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I like the nested state machines

@polygnomial polygnomial Mar 3, 2022

Copy link
Copy Markdown
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Theres definitely a way to clean this up with classes, but I dont think its worth it because there isn't any logic above them

@paxtonsc paxtonsc Mar 3, 2022

Copy link
Copy Markdown
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yeah. Some abstraction will probably be helpful so we do not end up repeating all this code on the way back.

@polygnomial polygnomial Mar 3, 2022

Copy link
Copy Markdown
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I was thinking with encoders it would be cool to have a replay functionality, where you just reverse all the way back to the start

STATE_ON_LINE, STATE_OFF_LEFT, STATE_OFF_RIGHT,
STATE_OFF_SLIGHT_RIGHT, STATE_OFF_SLIGHT_LEFT
} States_r;

// Zone defintions
typedef enum {
ZONE_LOAD, ZONE_A, ZONE_B, ZONE_C, ZONE_1, ZONE_2, ZONE_3, ZONE_4, ZONE_TARGET
} Zones_t;

/*----------------------------Function Prototypes------------*/
void checkGlobalEvents(void);
void checkForZoneChange(void);
void lineFollow(void);
void handleMoveForward(void);
void handleMoveBackward(void);
uint8_t TestForKey(void);
void RespToKey(void);
void handleRightTurn(void);
void handleLeftTurn(void);
void handleLoadState(void);
void handleUnloadState(void);
void handleNavTargetState(void);
void handleNavLoadState(void);
void changeStateTo(States_t);
void setup(void);
void loop(void);
void checkFlags(void);
void changeLineStateTo(States_r s);
void changeStateTo(States_t s);
void changeZoneTo(Zones_t z);
void setFlag(uint8_t flag);

/*---------------Module Variables---------------------------*/
Loading