update jessie

feelgood/feelgood.ino

@@ -1,6 +1,6 @@
 #include <PololuBuzzer.h>
 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-"

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);

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;