From 69fbef1734890fd02a8ddb211fc29a857653a612 Mon Sep 17 00:00:00 2001 From: maxlliiw Date: Fri, 22 Feb 2019 15:56:37 -0500 Subject: [PATCH 1/4] Created initMove --- feelgood/feelgood.ino | 2 +- sumo4/Robot.cpp | 62 ++++++++++++++++++++----------------------- sumo4/sumo4.ino | 3 +++ 3 files changed, 33 insertions(+), 34 deletions(-) diff --git a/feelgood/feelgood.ino b/feelgood/feelgood.ino index 201c4c8..739f5d3 100644 --- a/feelgood/feelgood.ino +++ b/feelgood/feelgood.ino @@ -1,6 +1,6 @@ #include PololuBuzzer buzzer; -const char FEELGOOD[] PROGMEM = "L4 V16 T130" +const char FEELGOOD[] PROGMEM = "L4 V9 T130" "O2 E- L8 R E-FG- R B R L4 B-" "O4 L8 F R L4 E-" "O2 R16 L4 A- L8 RA- BB-RG-R L4 E-" diff --git a/sumo4/Robot.cpp b/sumo4/Robot.cpp index cc503fd..031ea1f 100644 --- a/sumo4/Robot.cpp +++ b/sumo4/Robot.cpp @@ -7,10 +7,9 @@ class Robot{ int leftReading; int rightReading; const int MAX_SPEED = 400; - boolean firstAtLineFlag; - Timer atLineTimer; + public: - enum class State {init, wait, search, attack, evade, atLine}; + enum class State {init, initMove, wait, search, attack, evade, atLine}; State state = State::init; Zumo32U4ProximitySensors proxSensors; Zumo32U4Motors motors; @@ -22,6 +21,8 @@ class Robot{ Timer waitTimer; Timer turnTimer; + Timer atLineTimer; + Timer initMoveTimer; TurnSensor turnSen; @@ -45,7 +46,18 @@ class Robot{ Serial.print("Setup Complete - Waiting for Button"); ledRed(1); state = State::wait; - firstAtLineFlag = false; + } + void initMove(){ + initMoveTimer.startTimerC(); + if(initMoveTimer.timeElapsed() <= 250) + { + motors.setSpeeds(400,400); + } + else + { + turnDeg(220); + state = State::search; + } } void calibrateLineSensors(){ ledYellow(1); @@ -85,7 +97,7 @@ class Robot{ } if(waitTimer.timeElapsed() > 5000){ waitTimer.reset(); - state = State::search; + state = State::initMove; ledYellow(0); ledGreen(1); lcd.clear(); @@ -106,7 +118,7 @@ class Robot{ } void atLine(){ atLineTimer.startTimerC(); - if(atLineTimer.timeElapsed() < 175){ + if(atLineTimer.timeElapsed() < 200){ motors.setSpeeds(-400,-400); return; } @@ -121,37 +133,21 @@ class Robot{ //delay(200); while(turnSen.heading360 != toHeading) { - if(initialHeading + turn > 359) - { - toHeading = initialHeading + turn - 360; - } - else + if(turn > 0) { - toHeading = initialHeading + turn; + if(initialHeading + turn > 359) + { + toHeading = initialHeading + turn - 360; + } + else + { + toHeading = initialHeading + turn; + } + turnSen.turnSensorUpdate(); + motors.setSpeeds(400,-400); } - turnSen.turnSensorUpdate(); - motors.setSpeeds(-400,400); } - state = State::search; - - /*turnTimer.startTimerC(); - if(turnTimer.timeElapsed() < 100){ - motors.setSpeeds(-400,-400); - } - if(turnTimer.timeElapsed() >= 100 && turnTimer.timeElapsed() < 300){ - motors.setSpeeds(-300,300); - } - else if(turnTimer.timeElapsed() >= 300 && turnTimer.timeElapsed() < 500){ - motors.setSpeeds(300,300); - } - else{ - turnTimer.reset(); state = State::search; - }**/ - } - void gambit() - { - } void search(){ motors.setSpeeds(200,400); diff --git a/sumo4/sumo4.ino b/sumo4/sumo4.ino index d7d1ae6..cad852c 100644 --- a/sumo4/sumo4.ino +++ b/sumo4/sumo4.ino @@ -23,6 +23,9 @@ void loop() { case Robot::State::init: robot.init(); break; + case Robot::State::initMove: + robot.initMove(); + break; case Robot::State::wait: robot.wait(); break; From c6dc578d9db8511b8f62a1331035fb0a2e66b048 Mon Sep 17 00:00:00 2001 From: maxlliiw Date: Fri, 1 Mar 2019 16:15:38 -0500 Subject: [PATCH 2/4] Fix turnDeg --- feelgood/feelgood.ino | 2 +- sumo4/Robot.cpp | 53 +++++++++++++++++++++++-------------------- sumo4/sumo4.ino | 3 +++ 3 files changed, 32 insertions(+), 26 deletions(-) diff --git a/feelgood/feelgood.ino b/feelgood/feelgood.ino index 739f5d3..ee3c1df 100644 --- a/feelgood/feelgood.ino +++ b/feelgood/feelgood.ino @@ -1,6 +1,6 @@ #include PololuBuzzer buzzer; -const char FEELGOOD[] PROGMEM = "L4 V9 T130" +const char FEELGOOD[] PROGMEM = "L4 V8 T130" "O2 E- L8 R E-FG- R B R L4 B-" "O4 L8 F R L4 E-" "O2 R16 L4 A- L8 RA- BB-RG-R L4 E-" diff --git a/sumo4/Robot.cpp b/sumo4/Robot.cpp index 031ea1f..bf5b0d6 100644 --- a/sumo4/Robot.cpp +++ b/sumo4/Robot.cpp @@ -6,10 +6,12 @@ class Robot{ int lineReadings[5]; int leftReading; int rightReading; + uint32_t turn; + uint32_t toHeading = 90; const int MAX_SPEED = 400; public: - enum class State {init, initMove, wait, search, attack, evade, atLine}; + enum class State {init, initMove, wait, search, attack, evade, atLine, turnDeg}; State state = State::init; Zumo32U4ProximitySensors proxSensors; Zumo32U4Motors motors; @@ -33,10 +35,6 @@ class Robot{ turnSen.calculate360degreeheading(); leftReading = proxSensors.countsFrontWithLeftLeds(); rightReading = proxSensors.countsFrontWithRightLeds(); - Serial.print(turnSen.readableHeading); - Serial.print(" "); - Serial.print(turnSen.heading360); - Serial.print(" "); } void init(){ proxSensors.initThreeSensors(); @@ -55,8 +53,9 @@ class Robot{ } else { - turnDeg(220); - state = State::search; + turn = 60; + //calcToHeading(); + state = State::turnDeg; } } void calibrateLineSensors(){ @@ -122,32 +121,36 @@ class Robot{ motors.setSpeeds(-400,-400); return; } - turnDeg(100); + turn = 60; + //calcToHeading(); + state = State::turnDeg; } - void turnDeg(uint32_t turn){ + void calcToHeading() + { const uint32_t initialHeading = turnSen.heading360; - uint32_t toHeading = 0; + if(initialHeading + turn > 359) + { + toHeading = initialHeading + turn - 360; + } + else + { + toHeading = initialHeading + turn; + } + } + void turnDeg(){ //go backwards a little bit before turning //((turnTimer.timeElapsed() < 400) ? motors.setSpeeds(-400,-400) : motors.setSpeeds(0,0)); //motors.setSpeeds(-300,-300); //delay(200); - while(turnSen.heading360 != toHeading) + + int currentHeading = turnSen.readableHeading; + lcd.print(currentHeading); + motors.setSpeeds(400,-400); + + if(currentHeading == toHeading) { - if(turn > 0) - { - if(initialHeading + turn > 359) - { - toHeading = initialHeading + turn - 360; - } - else - { - toHeading = initialHeading + turn; - } - turnSen.turnSensorUpdate(); - motors.setSpeeds(400,-400); - } + state = State::search; } - state = State::search; } void search(){ motors.setSpeeds(200,400); diff --git a/sumo4/sumo4.ino b/sumo4/sumo4.ino index cad852c..ff6cfa5 100644 --- a/sumo4/sumo4.ino +++ b/sumo4/sumo4.ino @@ -26,6 +26,9 @@ void loop() { case Robot::State::initMove: robot.initMove(); break; + case Robot::State::turnDeg: + robot.turnDeg(); + break; case Robot::State::wait: robot.wait(); break; From 3fb2f10614c690d4aa24fa4894f30803e0846ad6 Mon Sep 17 00:00:00 2001 From: maxlliiw Date: Fri, 8 Mar 2019 14:25:09 -0500 Subject: [PATCH 3/4] TurnDeg State --- sumo4/Robot.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/sumo4/Robot.cpp b/sumo4/Robot.cpp index bf5b0d6..61bf4df 100644 --- a/sumo4/Robot.cpp +++ b/sumo4/Robot.cpp @@ -144,8 +144,7 @@ class Robot{ //delay(200); int currentHeading = turnSen.readableHeading; - lcd.print(currentHeading); - motors.setSpeeds(400,-400); + motors.setSpeeds(200,-200); if(currentHeading == toHeading) { From 1cbc01ba99ec92d2a12f3f6ae9c6e35b8fbc0000 Mon Sep 17 00:00:00 2001 From: maxlliiw Date: Fri, 8 Mar 2019 14:25:23 -0500 Subject: [PATCH 4/4] TurnDeg State --- sumo4/Robot.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/sumo4/Robot.cpp b/sumo4/Robot.cpp index 61bf4df..9e4f601 100644 --- a/sumo4/Robot.cpp +++ b/sumo4/Robot.cpp @@ -43,6 +43,7 @@ class Robot{ turnSen.turnSensorSetup(); Serial.print("Setup Complete - Waiting for Button"); ledRed(1); + turnSen.turnSensorReset(); state = State::wait; } void initMove(){ @@ -138,11 +139,6 @@ class Robot{ } } void turnDeg(){ - //go backwards a little bit before turning - //((turnTimer.timeElapsed() < 400) ? motors.setSpeeds(-400,-400) : motors.setSpeeds(0,0)); - //motors.setSpeeds(-300,-300); - //delay(200); - int currentHeading = turnSen.readableHeading; motors.setSpeeds(200,-200);