From 333aef23452d956647578a4d4eb7a9255e21d620 Mon Sep 17 00:00:00 2001 From: grantregen Date: Tue, 1 Mar 2022 21:49:57 -0800 Subject: [PATCH 01/10] begining of simple line follwing code --- Kabob_Code/src/Sensors/LineSensor.h | 2 +- Kabob_Code/src/StateMachine/StateMachine.cpp | 58 +++++++++++++++----- Kabob_Code/src/StateMachine/StateMachine.h | 4 +- 3 files changed, 48 insertions(+), 16 deletions(-) diff --git a/Kabob_Code/src/Sensors/LineSensor.h b/Kabob_Code/src/Sensors/LineSensor.h index 4c13cae..3b7db31 100644 --- a/Kabob_Code/src/Sensors/LineSensor.h +++ b/Kabob_Code/src/Sensors/LineSensor.h @@ -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; }; \ No newline at end of file diff --git a/Kabob_Code/src/StateMachine/StateMachine.cpp b/Kabob_Code/src/StateMachine/StateMachine.cpp index 30704dc..a22b30a 100644 --- a/Kabob_Code/src/StateMachine/StateMachine.cpp +++ b/Kabob_Code/src/StateMachine/StateMachine.cpp @@ -23,7 +23,8 @@ void setup() { void loop() { checkGlobalEvents(); - shephard.activity(); + checkLineSensors(); + // shephard.activity(); switch(state) { case STATE_IDLE: @@ -32,6 +33,15 @@ void loop() { case STATE_LOAD: handleLoadState(); break; + case STATE_ON_LINE: + shephard.chassis.move_forward_at_speed(200); + break; + case STATE_OFF_RIGHT: + shephard.chassis.veer_forward(250, 180); + break; + case STATE_OFF_LEFT: + shephard.chassis.veer_forward(180, 250); + break; case STATE_NAV_TARGET: handleNavTargetState(); break; @@ -44,17 +54,17 @@ void loop() { 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; - } - + // unsigned long current_millis = millis(); + // if (current_millis - 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 = current_millis; + // } } void handleLoadState(void) { @@ -70,7 +80,7 @@ void handleLoadState(void) { void handleNavTargetState(void){ shephard.chassis.move_forward_at_speed(200); - if (millis() - state_time> 5000) { + if (millis() - state_time> 5000){ changeStateTo(STATE_IDLE); } } @@ -78,12 +88,29 @@ void handleNavTargetState(void){ void changeStateTo(States_t s) { state = s; state_time = millis(); - Serial.println("New state = " + s); + //Serial.println("New state = " + s); } void checkGlobalEvents(void) { if (TestForKey()) RespToKey(); } + +void checkLineSensors(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 (center_middle && !center_right && !center_left){ + changeStateTo(STATE_ON_LINE); + } + else if (!center_middle && center_right && !center_left) { + changeStateTo(STATE_OFF_LEFT); + } + else if (!center_middle && !center_right && center_left) { + changeStateTo(STATE_OFF_RIGHT); + } +} uint8_t TestForKey(void) { uint8_t KeyEventOccurred; @@ -102,6 +129,9 @@ void RespToKey(void) { case ' ': state = STATE_IDLE; break; + case 'r': + changeStateTo(STATE_NAV_TARGET); + break; default: break; } diff --git a/Kabob_Code/src/StateMachine/StateMachine.h b/Kabob_Code/src/StateMachine/StateMachine.h index 006e6d2..f2d296f 100644 --- a/Kabob_Code/src/StateMachine/StateMachine.h +++ b/Kabob_Code/src/StateMachine/StateMachine.h @@ -9,11 +9,13 @@ /*---------------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, STATE_ON_LINE, STATE_OFF_LEFT, STATE_OFF_RIGHT } States_t; /*----------------------------Function Prototypes------------*/ void checkGlobalEvents(void); +void checkLineSensors(void); void handleMoveForward(void); void handleMoveBackward(void); uint8_t TestForKey(void); From 580fd9429475fadc9696ab2fa72642f6c74b0f86 Mon Sep 17 00:00:00 2001 From: grantregen Date: Tue, 1 Mar 2022 22:22:42 -0800 Subject: [PATCH 02/10] code can now follow straight line, not turns yet --- Kabob_Code/src/StateMachine/StateMachine.cpp | 39 ++++++++++++++++++-- Kabob_Code/src/StateMachine/StateMachine.h | 5 ++- 2 files changed, 40 insertions(+), 4 deletions(-) diff --git a/Kabob_Code/src/StateMachine/StateMachine.cpp b/Kabob_Code/src/StateMachine/StateMachine.cpp index a22b30a..ffbcd92 100644 --- a/Kabob_Code/src/StateMachine/StateMachine.cpp +++ b/Kabob_Code/src/StateMachine/StateMachine.cpp @@ -4,9 +4,15 @@ States_t state; unsigned long state_time; unsigned long serial_time; +unsigned long curr_time; uint8_t servoPin = 6; uint8_t servoPos = 0; +uint8_t flagLeftLine = 0; +uint8_t flagRightLine = 0; + +unsigned long flag_time; + void setup() { // put your setup code here, to run once: Serial.begin(9600); @@ -15,6 +21,8 @@ void setup() { //timer serial_time = millis(); state_time = millis(); + flag_time = millis(); + curr_time = millis(); state = STATE_IDLE; @@ -22,8 +30,10 @@ void setup() { } void loop() { + curr_time = millis(); checkGlobalEvents(); checkLineSensors(); + checkFlags(); // shephard.activity(); switch(state) { @@ -39,9 +49,15 @@ void loop() { case STATE_OFF_RIGHT: shephard.chassis.veer_forward(250, 180); break; + case STATE_OFF_SLIGHT_RIGHT: + shephard.chassis.veer_forward(250, 180); + break; case STATE_OFF_LEFT: shephard.chassis.veer_forward(180, 250); break; + case STATE_OFF_SLIGHT_LEFT: + shephard.chassis.veer_forward(180, 250); + break; case STATE_NAV_TARGET: handleNavTargetState(); break; @@ -86,21 +102,30 @@ void handleNavTargetState(void){ } void changeStateTo(States_t s) { - state = s; - state_time = millis(); - //Serial.println("New state = " + s); + if (s != state) { + state = s; + state_time = millis(); + } } void checkGlobalEvents(void) { if (TestForKey()) RespToKey(); } +void checkFlags(void) { + if (curr_time - flag_time > FLAG_TIME) { + flagLeftLine = 0; + flagRightLine = 0; + } +} + void checkLineSensors(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(); + // primary line sensing states if (center_middle && !center_right && !center_left){ changeStateTo(STATE_ON_LINE); } @@ -110,6 +135,14 @@ void checkLineSensors(void) { else if (!center_middle && !center_right && center_left) { changeStateTo(STATE_OFF_RIGHT); } + else if (center_middle && !center_right && center_left) { + changeStateTo(STATE_OFF_SLIGHT_RIGHT); + } + else if (center_middle && center_right && !center_left) { + changeStateTo(STATE_OFF_SLIGHT_LEFT); + } + // line divides in map + // TO BE IMPLIMENTED HERE USING FLAGS } uint8_t TestForKey(void) { diff --git a/Kabob_Code/src/StateMachine/StateMachine.h b/Kabob_Code/src/StateMachine/StateMachine.h index f2d296f..b3edb57 100644 --- a/Kabob_Code/src/StateMachine/StateMachine.h +++ b/Kabob_Code/src/StateMachine/StateMachine.h @@ -6,11 +6,13 @@ #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_ON_LINE, STATE_OFF_LEFT, STATE_OFF_RIGHT + STATE_NAV_LOAD, STATE_ON_LINE, STATE_OFF_LEFT, STATE_OFF_RIGHT, + STATE_OFF_SLIGHT_RIGHT, STATE_OFF_SLIGHT_LEFT } States_t; /*----------------------------Function Prototypes------------*/ @@ -27,5 +29,6 @@ void handleNavTargetState(void); void changeStateTo(States_t); void setup(void); void loop(void); +void checkFlags(void); /*---------------Module Variables---------------------------*/ From 231e1399e452d4ac40393ffb93f78147359cab9c Mon Sep 17 00:00:00 2001 From: Paxton Scott Date: Wed, 2 Mar 2022 10:54:48 -0800 Subject: [PATCH 03/10] code should hopefully get us to the place were we turn 90 degrees --- Kabob_Code/src/StateMachine/StateMachine.cpp | 158 +++++++++++++------ Kabob_Code/src/StateMachine/StateMachine.h | 19 ++- 2 files changed, 127 insertions(+), 50 deletions(-) diff --git a/Kabob_Code/src/StateMachine/StateMachine.cpp b/Kabob_Code/src/StateMachine/StateMachine.cpp index ffbcd92..c7347bf 100644 --- a/Kabob_Code/src/StateMachine/StateMachine.cpp +++ b/Kabob_Code/src/StateMachine/StateMachine.cpp @@ -1,10 +1,14 @@ #include "StateMachine.h" States_t state; +States_r line_state; +Zones_t zone; unsigned long state_time; unsigned long serial_time; unsigned long curr_time; +unsigned long zone_time; + uint8_t servoPin = 6; uint8_t servoPos = 0; @@ -19,12 +23,13 @@ void setup() { while(!Serial); //timer + curr_time = millis(); serial_time = millis(); state_time = millis(); flag_time = millis(); - curr_time = millis(); - state = STATE_IDLE; + // currently not doing anything + state = STATE_NAV_TARGET; Serial.println("Setup Complete!"); } @@ -32,10 +37,11 @@ void setup() { void loop() { curr_time = millis(); checkGlobalEvents(); - checkLineSensors(); checkFlags(); // shephard.activity(); + // TO LINE FOLLOW: Must be in state = state_nav_target + switch(state) { case STATE_IDLE: shephard.chassis.stop(); @@ -43,21 +49,6 @@ void loop() { case STATE_LOAD: handleLoadState(); break; - case STATE_ON_LINE: - shephard.chassis.move_forward_at_speed(200); - break; - case STATE_OFF_RIGHT: - shephard.chassis.veer_forward(250, 180); - break; - case STATE_OFF_SLIGHT_RIGHT: - shephard.chassis.veer_forward(250, 180); - break; - case STATE_OFF_LEFT: - shephard.chassis.veer_forward(180, 250); - break; - case STATE_OFF_SLIGHT_LEFT: - shephard.chassis.veer_forward(180, 250); - break; case STATE_NAV_TARGET: handleNavTargetState(); break; @@ -70,17 +61,18 @@ void loop() { 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("---------------"); - // 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 = current_millis; - // } + //debug for line sensors + if (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.println("In Zone" + zone); + serial_time = curr_time; + } } void handleLoadState(void) { @@ -95,16 +87,33 @@ void handleLoadState(void) { } void handleNavTargetState(void){ - shephard.chassis.move_forward_at_speed(200); - if (millis() - state_time> 5000){ - changeStateTo(STATE_IDLE); - } -} - -void changeStateTo(States_t s) { - if (s != state) { - state = s; - state_time = millis(); + //shephard.chassis.move_forward_at_speed(200); + //if (millis() - state_time> 5000){ + // changeStateTo(STATE_IDLE); + //} + switch(zone) { + case ZONE_LOAD: + break; + case ZONE_1: + lineFollow(); + break; + case ZONE_2: + lineFollow(); + break; + case ZONE_3: + // turn 90 degrees and then line follow + if (curr_time - zone_time < 1000) { + shephard.chassis.turn_right(200); + } else { + lineFollow(); + } + break; + case ZONE_4: + break; + case ZONE_TARGET: + break; + default: + Serial.println("Zone has more states than Paxton thought"); } } @@ -119,30 +128,62 @@ void checkFlags(void) { } } -void checkLineSensors(void) { +void checkForZoneChange(void) { uint8_t left = shephard.sensors.line.left.read(); uint8_t right = shephard.sensors.line.right.read(); + if (left && zone == ZONE_1) { + flagLeftLine = true; + flag_time = millis(); + changeZoneTo(ZONE_2); + } else if (right && zone == ZONE_2) { + flagRightLine = true; + flag_time = millis(); + changeZoneTo(ZONE_3); + } +} + + +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){ - changeStateTo(STATE_ON_LINE); + changeLineStateTo(STATE_ON_LINE); } else if (!center_middle && center_right && !center_left) { - changeStateTo(STATE_OFF_LEFT); + changeLineStateTo(STATE_OFF_LEFT); } else if (!center_middle && !center_right && center_left) { - changeStateTo(STATE_OFF_RIGHT); + changeLineStateTo(STATE_OFF_RIGHT); } else if (center_middle && !center_right && center_left) { - changeStateTo(STATE_OFF_SLIGHT_RIGHT); + changeLineStateTo(STATE_OFF_SLIGHT_RIGHT); } else if (center_middle && center_right && !center_left) { - changeStateTo(STATE_OFF_SLIGHT_LEFT); + changeLineStateTo(STATE_OFF_SLIGHT_LEFT); } - // line divides in map - // TO BE IMPLIMENTED HERE USING FLAGS + + switch(line_state) { + case STATE_ON_LINE: + shephard.chassis.move_forward_at_speed(200); + break; + case STATE_OFF_RIGHT: + shephard.chassis.veer_forward(250, 180); + break; + case STATE_OFF_SLIGHT_RIGHT: + shephard.chassis.veer_forward(250, 180); + break; + case STATE_OFF_LEFT: + shephard.chassis.veer_forward(180, 250); + break; + case STATE_OFF_SLIGHT_LEFT: + shephard.chassis.veer_forward(180, 250); + break; + default: + Serial.println("humm"); + } + } uint8_t TestForKey(void) { @@ -151,6 +192,26 @@ uint8_t TestForKey(void) { return KeyEventOccurred; } + +void changeStateTo(States_t s) { + 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; + } +} + void RespToKey(void) { uint8_t theKey; theKey = Serial.read(); @@ -169,3 +230,6 @@ void RespToKey(void) { break; } } + + + diff --git a/Kabob_Code/src/StateMachine/StateMachine.h b/Kabob_Code/src/StateMachine/StateMachine.h index b3edb57..0a2f5f6 100644 --- a/Kabob_Code/src/StateMachine/StateMachine.h +++ b/Kabob_Code/src/StateMachine/StateMachine.h @@ -11,13 +11,23 @@ /*---------------State Definitions--------------------------*/ typedef enum { STATE_IDLE, STATE_LOAD, STATE_NAV_TARGET, STATE_UNLOAD, - STATE_NAV_LOAD, STATE_ON_LINE, STATE_OFF_LEFT, STATE_OFF_RIGHT, - STATE_OFF_SLIGHT_RIGHT, STATE_OFF_SLIGHT_LEFT + STATE_NAV_LOAD, } States_t; +typedef enum { + 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_1, ZONE_2, ZONE_3, ZONE_4, ZONE_TARGET +} Zones_t; + /*----------------------------Function Prototypes------------*/ void checkGlobalEvents(void); -void checkLineSensors(void); +void checkForZoneChange(void); +void lineFollow(void); void handleMoveForward(void); void handleMoveBackward(void); uint8_t TestForKey(void); @@ -30,5 +40,8 @@ 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); /*---------------Module Variables---------------------------*/ From 9943385757e52400ef36703bc114f782ba960041 Mon Sep 17 00:00:00 2001 From: Paxton Scott Date: Wed, 2 Mar 2022 11:02:26 -0800 Subject: [PATCH 04/10] adding a couple comments --- Kabob_Code/src/StateMachine/StateMachine.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Kabob_Code/src/StateMachine/StateMachine.cpp b/Kabob_Code/src/StateMachine/StateMachine.cpp index c7347bf..471ed61 100644 --- a/Kabob_Code/src/StateMachine/StateMachine.cpp +++ b/Kabob_Code/src/StateMachine/StateMachine.cpp @@ -28,7 +28,6 @@ void setup() { state_time = millis(); flag_time = millis(); - // currently not doing anything state = STATE_NAV_TARGET; Serial.println("Setup Complete!"); @@ -109,8 +108,10 @@ void handleNavTargetState(void){ } break; case ZONE_4: + // TO IMPLEMENT break; case ZONE_TARGET: + // TO IMPLEMENT break; default: Serial.println("Zone has more states than Paxton thought"); From d00557dee2a196c8f556cacecb3320d52d7ca586 Mon Sep 17 00:00:00 2001 From: Paxton Scott Date: Wed, 2 Mar 2022 13:16:46 -0800 Subject: [PATCH 05/10] robot moving from one side of the course to the other --- Kabob_Code/src/StateMachine/StateMachine.cpp | 20 ++++++++++++++++---- Kabob_Code/src/Test/Test.cpp | 2 +- Kabob_Code/src/Test/Test.h | 1 + 3 files changed, 18 insertions(+), 5 deletions(-) diff --git a/Kabob_Code/src/StateMachine/StateMachine.cpp b/Kabob_Code/src/StateMachine/StateMachine.cpp index 471ed61..6701985 100644 --- a/Kabob_Code/src/StateMachine/StateMachine.cpp +++ b/Kabob_Code/src/StateMachine/StateMachine.cpp @@ -29,6 +29,7 @@ void setup() { flag_time = millis(); state = STATE_NAV_TARGET; + zone = ZONE_1; Serial.println("Setup Complete!"); } @@ -37,6 +38,7 @@ void loop() { curr_time = millis(); checkGlobalEvents(); checkFlags(); + checkForZoneChange(); // shephard.activity(); // TO LINE FOLLOW: Must be in state = state_nav_target @@ -69,7 +71,6 @@ void loop() { //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.println("In Zone" + zone); serial_time = curr_time; } } @@ -89,7 +90,8 @@ 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; @@ -101,14 +103,19 @@ void handleNavTargetState(void){ break; case ZONE_3: // turn 90 degrees and then line follow - if (curr_time - zone_time < 1000) { + if (t > 1000 && t < 5000) { shephard.chassis.turn_right(200); } else { lineFollow(); } break; case ZONE_4: - // TO IMPLEMENT + // turn 90 degrees and then line follow + if (t < 2500) { + shephard.chassis.turn_left(200); + } else { + lineFollow(); + } break; case ZONE_TARGET: // TO IMPLEMENT @@ -132,6 +139,7 @@ void checkFlags(void) { void checkForZoneChange(void) { uint8_t left = shephard.sensors.line.left.read(); uint8_t right = shephard.sensors.line.right.read(); + //Serial.println(left); if (left && zone == ZONE_1) { flagLeftLine = true; flag_time = millis(); @@ -140,6 +148,8 @@ void checkForZoneChange(void) { flagRightLine = true; flag_time = millis(); changeZoneTo(ZONE_3); + } else if (left && zone == ZONE_3 && curr_time - zone_time > 5000) { + changeZoneTo(ZONE_4); } } @@ -210,6 +220,8 @@ void changeLineStateTo(States_r s) { void changeZoneTo(Zones_t z) { if (z != zone) { zone = z; + zone_time = millis(); + Serial.println("In new zone " + z); } } diff --git a/Kabob_Code/src/Test/Test.cpp b/Kabob_Code/src/Test/Test.cpp index de8b1cd..4a5548f 100644 --- a/Kabob_Code/src/Test/Test.cpp +++ b/Kabob_Code/src/Test/Test.cpp @@ -4,6 +4,6 @@ void setup(){ Serial.begin(9600); } void loop(void){ - Serial.println("Currently doing some testing..."); + shephard.chassis.turn_right(200); delay(1000); } \ No newline at end of file diff --git a/Kabob_Code/src/Test/Test.h b/Kabob_Code/src/Test/Test.h index 4ab92df..141a578 100644 --- a/Kabob_Code/src/Test/Test.h +++ b/Kabob_Code/src/Test/Test.h @@ -2,3 +2,4 @@ #include #include +#include "System.h" \ No newline at end of file From 32b20cdd0963ee7ea2e54a4def2ccf3e8e259592 Mon Sep 17 00:00:00 2001 From: Paxton Scott Date: Wed, 2 Mar 2022 13:18:06 -0800 Subject: [PATCH 06/10] time change --- Kabob_Code/src/StateMachine/StateMachine.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Kabob_Code/src/StateMachine/StateMachine.cpp b/Kabob_Code/src/StateMachine/StateMachine.cpp index 6701985..9c8e6bd 100644 --- a/Kabob_Code/src/StateMachine/StateMachine.cpp +++ b/Kabob_Code/src/StateMachine/StateMachine.cpp @@ -111,7 +111,7 @@ void handleNavTargetState(void){ break; case ZONE_4: // turn 90 degrees and then line follow - if (t < 2500) { + if (t < 3000) { shephard.chassis.turn_left(200); } else { lineFollow(); From c622d4e21bcd9a178af8a514fafb62842877feb2 Mon Sep 17 00:00:00 2001 From: Paxton Scott Date: Wed, 2 Mar 2022 16:27:48 -0800 Subject: [PATCH 07/10] adding complexity to line following. Next step clean up code --- Kabob_Code/src/StateMachine/StateMachine.cpp | 70 ++++++++++++++------ Kabob_Code/src/StateMachine/StateMachine.h | 3 + Kabob_Code/src/Test/Test.cpp | 7 +- 3 files changed, 59 insertions(+), 21 deletions(-) diff --git a/Kabob_Code/src/StateMachine/StateMachine.cpp b/Kabob_Code/src/StateMachine/StateMachine.cpp index 9c8e6bd..94248df 100644 --- a/Kabob_Code/src/StateMachine/StateMachine.cpp +++ b/Kabob_Code/src/StateMachine/StateMachine.cpp @@ -1,5 +1,7 @@ #include "StateMachine.h" +#define VELOCITY 200 + States_t state; States_r line_state; Zones_t zone; @@ -21,15 +23,15 @@ 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_NAV_TARGET; - zone = ZONE_1; + changeStateTo(STATE_LOAD); Serial.println("Setup Complete!"); } @@ -39,7 +41,6 @@ void loop() { checkGlobalEvents(); checkFlags(); checkForZoneChange(); - // shephard.activity(); // TO LINE FOLLOW: Must be in state = state_nav_target @@ -54,9 +55,10 @@ 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..."); @@ -76,16 +78,32 @@ void loop() { } 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 < 6000) { + shephard.chassis.move_forward_at_speed(VELOCITY); + } else if (timeInState > 6000) { changeStateTo(STATE_NAV_TARGET); + changeZoneTo(ZONE_1); } } +void handleUnloadState(void) { + int timeInState = curr_time - state_time; + if (timeInState < 1000) { + shephard.claw.open(); + } else if (timeInState > 1000 && timeInState < 4000) { + shephard.chassis.move_backward_at_speed(VELOCITY); + } 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){ @@ -118,13 +136,21 @@ void handleNavTargetState(void){ } break; case ZONE_TARGET: - // TO IMPLEMENT + 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 handleNavLoadState(void) { + shephard.chassis.stop(); +} + void checkGlobalEvents(void) { if (TestForKey()) RespToKey(); } @@ -141,15 +167,16 @@ void checkForZoneChange(void) { uint8_t right = shephard.sensors.line.right.read(); //Serial.println(left); if (left && zone == ZONE_1) { - flagLeftLine = true; - flag_time = millis(); + setFlag(flagLeftLine); changeZoneTo(ZONE_2); } else if (right && zone == ZONE_2) { - flagRightLine = true; - flag_time = millis(); + setFlag(flagRightLine); changeZoneTo(ZONE_3); } else if (left && zone == ZONE_3 && curr_time - zone_time > 5000) { changeZoneTo(ZONE_4); + setFlag(flagLeftLine); + } else if (left && zone == ZONE_4 && curr_time - zone_time > 6000) { + changeZoneTo(ZONE_TARGET); } } @@ -168,28 +195,28 @@ void lineFollow(void) { else if (!center_middle && !center_right && center_left) { changeLineStateTo(STATE_OFF_RIGHT); } - else if (center_middle && !center_right && center_left) { + else if (center_middle && !center_right && center_left && !flagLeftLine) { changeLineStateTo(STATE_OFF_SLIGHT_RIGHT); } - else if (center_middle && center_right && !center_left) { + 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(200); + 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, 180); + 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(180, 250); + shephard.chassis.veer_forward(200, 250); break; default: Serial.println("humm"); @@ -225,6 +252,11 @@ void changeZoneTo(Zones_t z) { } } +void setFlag(uint8_t flag){ + flag = true; + flag_time = curr_time; +} + void RespToKey(void) { uint8_t theKey; theKey = Serial.read(); diff --git a/Kabob_Code/src/StateMachine/StateMachine.h b/Kabob_Code/src/StateMachine/StateMachine.h index 0a2f5f6..2d83038 100644 --- a/Kabob_Code/src/StateMachine/StateMachine.h +++ b/Kabob_Code/src/StateMachine/StateMachine.h @@ -35,7 +35,9 @@ 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); @@ -43,5 +45,6 @@ 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---------------------------*/ diff --git a/Kabob_Code/src/Test/Test.cpp b/Kabob_Code/src/Test/Test.cpp index 4a5548f..c27f9ce 100644 --- a/Kabob_Code/src/Test/Test.cpp +++ b/Kabob_Code/src/Test/Test.cpp @@ -4,6 +4,9 @@ void setup(){ Serial.begin(9600); } void loop(void){ - shephard.chassis.turn_right(200); - delay(1000); + shephard.chassis.turn_left_at_speed(255); + delay(6000); + shephard.chassis.stop(); + delay(2000); + } \ No newline at end of file From 2916429c276ada31809d57dc3b40970145a145c7 Mon Sep 17 00:00:00 2001 From: Paxton Scott Date: Wed, 2 Mar 2022 16:43:57 -0800 Subject: [PATCH 08/10] couple small cleanup things --- Kabob_Code/src/StateMachine/StateMachine.cpp | 44 ++++++++++---------- 1 file changed, 23 insertions(+), 21 deletions(-) diff --git a/Kabob_Code/src/StateMachine/StateMachine.cpp b/Kabob_Code/src/StateMachine/StateMachine.cpp index 94248df..e752387 100644 --- a/Kabob_Code/src/StateMachine/StateMachine.cpp +++ b/Kabob_Code/src/StateMachine/StateMachine.cpp @@ -11,12 +11,11 @@ unsigned long serial_time; unsigned long curr_time; unsigned long zone_time; -uint8_t servoPin = 6; -uint8_t servoPos = 0; - uint8_t flagLeftLine = 0; uint8_t flagRightLine = 0; +uint8_t DEBUG = false; + unsigned long flag_time; void setup() { @@ -42,8 +41,6 @@ void loop() { checkFlags(); checkForZoneChange(); - // TO LINE FOLLOW: Must be in state = state_nav_target - switch(state) { case STATE_IDLE: shephard.chassis.stop(); @@ -65,14 +62,14 @@ void loop() { } //debug for line sensors - if (curr_time - serial_time > MILLISECONDS(1/PRINT_FREQUENCY) ){ + 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.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; } } @@ -83,9 +80,11 @@ void handleLoadState(void) { shephard.claw.open(); } else if (timeInState > 1000 && timeInState < 2000) { shephard.claw.close(); - } else if (timeInState > 2000 && timeInState < 6000) { + } 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 > 6000) { + } else if (timeInState > 3000) { changeStateTo(STATE_NAV_TARGET); changeZoneTo(ZONE_1); } @@ -95,20 +94,23 @@ void handleUnloadState(void) { int timeInState = curr_time - state_time; if (timeInState < 1000) { shephard.claw.open(); - } else if (timeInState > 1000 && timeInState < 4000) { + } + + // back up after depositing ball + else if (timeInState > 1000 && timeInState < 4000) { shephard.chassis.move_backward_at_speed(VELOCITY); - } else if (timeInState > 4000 && timeInState < 10000) { + } + // about 6 secounds to rotate 180 degrees + else if (timeInState > 4000 && timeInState < 10000) { shephard.chassis.turn_right_at_speed(255); - } else if (timeInState > 9000) { + } + + 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: From e4cd6cb853c4c239a0ebc773d54d1f9fb10f4c4c Mon Sep 17 00:00:00 2001 From: d-arcymarie Date: Wed, 2 Mar 2022 18:36:08 -0800 Subject: [PATCH 09/10] line follow loading zone update --- Kabob_Code/src/StateMachine/StateMachine.cpp | 38 +++++++++++++++++--- Kabob_Code/src/StateMachine/StateMachine.h | 2 +- 2 files changed, 35 insertions(+), 5 deletions(-) diff --git a/Kabob_Code/src/StateMachine/StateMachine.cpp b/Kabob_Code/src/StateMachine/StateMachine.cpp index e752387..3d4dcfa 100644 --- a/Kabob_Code/src/StateMachine/StateMachine.cpp +++ b/Kabob_Code/src/StateMachine/StateMachine.cpp @@ -86,7 +86,7 @@ void handleLoadState(void) { shephard.chassis.move_forward_at_speed(VELOCITY); } else if (timeInState > 3000) { changeStateTo(STATE_NAV_TARGET); - changeZoneTo(ZONE_1); + changeZoneTo(ZONE_A); } } @@ -114,9 +114,30 @@ void handleNavTargetState(void){ 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: - lineFollow(); + if (t < 4000) { + shephard.chassis.move_forward_at_speed(200); + } else { + lineFollow(); + } break; case ZONE_2: lineFollow(); @@ -167,8 +188,17 @@ void checkFlags(void) { void checkForZoneChange(void) { uint8_t left = shephard.sensors.line.left.read(); uint8_t right = shephard.sensors.line.right.read(); - //Serial.println(left); - if (left && zone == ZONE_1) { + 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"); + if (left && (zone == ZONE_A)) { + changeZoneTo(ZONE_B); + } else if (left && (zone == ZONE_B)) { + changeZoneTo(ZONE_C); + } else if (right && (zone == ZONE_C)) { + changeZoneTo(ZONE_1); + } else if (left && zone == ZONE_1) { setFlag(flagLeftLine); changeZoneTo(ZONE_2); } else if (right && zone == ZONE_2) { diff --git a/Kabob_Code/src/StateMachine/StateMachine.h b/Kabob_Code/src/StateMachine/StateMachine.h index 2d83038..a5f6b35 100644 --- a/Kabob_Code/src/StateMachine/StateMachine.h +++ b/Kabob_Code/src/StateMachine/StateMachine.h @@ -21,7 +21,7 @@ typedef enum { // Zone defintions typedef enum { - ZONE_LOAD, ZONE_1, ZONE_2, ZONE_3, ZONE_4, ZONE_TARGET + ZONE_LOAD, ZONE_A, ZONE_B, ZONE_C, ZONE_1, ZONE_2, ZONE_3, ZONE_4, ZONE_TARGET } Zones_t; /*----------------------------Function Prototypes------------*/ From 2d7cdf6851186ec5d712ba90bf44d0717b6db083 Mon Sep 17 00:00:00 2001 From: d-arcymarie Date: Wed, 2 Mar 2022 21:45:13 -0800 Subject: [PATCH 10/10] small line follow ABC update --- Kabob_Code/src/StateMachine/StateMachine.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/Kabob_Code/src/StateMachine/StateMachine.cpp b/Kabob_Code/src/StateMachine/StateMachine.cpp index 3d4dcfa..d97b681 100644 --- a/Kabob_Code/src/StateMachine/StateMachine.cpp +++ b/Kabob_Code/src/StateMachine/StateMachine.cpp @@ -191,12 +191,13 @@ void checkForZoneChange(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(); - if (left) Serial.println("left sensor tripped"); + // if (left) Serial.println("left sensor tripped"); + Serial.println(zone); if (left && (zone == ZONE_A)) { changeZoneTo(ZONE_B); - } else if (left && (zone == ZONE_B)) { + } else if (left && zone == ZONE_B && curr_time - zone_time > 3000) { changeZoneTo(ZONE_C); - } else if (right && (zone == ZONE_C)) { + } else if (right && zone == ZONE_C && curr_time - zone_time > 5000) { changeZoneTo(ZONE_1); } else if (left && zone == ZONE_1) { setFlag(flagLeftLine);