diff --git a/feelgood/feelgood.ino b/feelgood/feelgood.ino index 201c4c8..ee3c1df 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 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 cc503fd..9e4f601 100644 --- a/sumo4/Robot.cpp +++ b/sumo4/Robot.cpp @@ -6,11 +6,12 @@ class Robot{ int lineReadings[5]; int leftReading; int rightReading; + uint32_t turn; + uint32_t toHeading = 90; 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, turnDeg}; State state = State::init; Zumo32U4ProximitySensors proxSensors; Zumo32U4Motors motors; @@ -22,6 +23,8 @@ class Robot{ Timer waitTimer; Timer turnTimer; + Timer atLineTimer; + Timer initMoveTimer; TurnSensor turnSen; @@ -32,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(); @@ -44,8 +43,21 @@ class Robot{ turnSen.turnSensorSetup(); Serial.print("Setup Complete - Waiting for Button"); ledRed(1); + turnSen.turnSensorReset(); state = State::wait; - firstAtLineFlag = false; + } + void initMove(){ + initMoveTimer.startTimerC(); + if(initMoveTimer.timeElapsed() <= 250) + { + motors.setSpeeds(400,400); + } + else + { + turn = 60; + //calcToHeading(); + state = State::turnDeg; + } } 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,52 +118,34 @@ class Robot{ } void atLine(){ atLineTimer.startTimerC(); - if(atLineTimer.timeElapsed() < 175){ + if(atLineTimer.timeElapsed() < 200){ 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; - //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) + if(initialHeading + turn > 359) { - if(initialHeading + turn > 359) - { - toHeading = initialHeading + turn - 360; - } - else - { - toHeading = initialHeading + turn; - } - turnSen.turnSensorUpdate(); - motors.setSpeeds(-400,400); + toHeading = initialHeading + turn - 360; + } + else + { + toHeading = initialHeading + turn; } - 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 turnDeg(){ + int currentHeading = turnSen.readableHeading; + motors.setSpeeds(200,-200); + + if(currentHeading == toHeading) + { + state = State::search; + } } void search(){ motors.setSpeeds(200,400); diff --git a/sumo4/sumo4.ino b/sumo4/sumo4.ino index d7d1ae6..ff6cfa5 100644 --- a/sumo4/sumo4.ino +++ b/sumo4/sumo4.ino @@ -23,6 +23,12 @@ void loop() { case Robot::State::init: robot.init(); break; + case Robot::State::initMove: + robot.initMove(); + break; + case Robot::State::turnDeg: + robot.turnDeg(); + break; case Robot::State::wait: robot.wait(); break;