Added data loggers to Track functions and state changes in autonomous,...

Autonomous.cpp

@@ -3,10 +3,14 @@
 #include "ports.h"
 #include "612.h"
 #include "Timer.h"
+#include "Shooter.h"
+#include "ports.h"
+#include <fstream>
+
 
 std::string AUTO_TABLE_NAME="PCVision";
 
-Autonomous::Autonomous(main_robot* r):table(NetworkTable::GetTable(AUTO_TABLE_NAME))
+Autonomous::Autonomous(main_robot* r): log("testLog.txt", std::ofstream::out, std::ofstream::app), table(NetworkTable::GetTable(AUTO_TABLE_NAME))
 {
     robot = r;
     timer = new Timer();
@@ -24,6 +28,13 @@ bool Autonomous::moveForward(double dist)
     if (previousStage != stage)
     {
         robot->drive->autoDrive(dist);
+        static int logCap;
+        if(logCap % 20 == 0)
+        {
+            log << "function robot->drive->autoDrive has been called\n";
+            log.flush();
+        }
+        logCap++;
     }
     return !(robot->drive->isAuto());
 }
@@ -33,6 +44,13 @@ bool Autonomous::tilt(double angle)        // needs to tilt a certain degrees, p
     if(previousStage != stage)
     {
         robot->shoot->pitchAngle(angle);
+        static int logCap;
+        if(logCap % 20 == 0)
+        {
+            log << "function robot->shoot->pitchAngle has been called\n";
+            log.flush();
+        }
+        logCap++;
     }
     if(!robot->shoot->accelWorking)
     {
@@ -47,6 +65,13 @@ bool Autonomous::wormPull()
     {
         robot->shoot->autoPulling=true;
         robot->shoot->wormPull();
+        static int logCap;
+        if(logCap % 20 == 0)
+        {
+            log << "function robot->shoot->wormPull has been called\n";
+            log.flush();
+        }
+        logCap++;
     }
     bool wormDone = robot->shoot->wormDone();
     if(wormDone)
@@ -70,6 +95,13 @@ bool Autonomous::smartFire()
     if (previousStage != stage)
     {
         robot->shoot->smartFire();
+        static int logCap;
+        if(logCap % 20 == 0)
+        {
+            log << "function robot->shoot->smartFire has been called\n";
+            log.flush();
+        }
+        logCap++;
     }
     return !robot->shoot->smartFiring;
 }
@@ -109,15 +141,46 @@ void Autonomous::updateHighGoal()
     static int output=0;
     switch (stage)
     {
+	case DRIVE_AIM_WINCH:
+            bool driveDone=moveForward(DISTANCE);
+            bool aimDone=tilt(HIGHGOAL_AUTOANGLE);
+            bool winchDone=wormPull();
+            if(output%20==0)
+            {
+                printf("drive: %i, aim: %i, winch: %i\n",driveDone,aimDone,winchDone);
+                static int logCap;
+                if(logCap % 20 == 0)
+                {
+                    log << "drive:" << driveDone << aimDone << winchDone << "\n";
+                    log.flush();
+                }
+                logCap++;
+            }
         case IDLE:
             printf("AUTO switch to COARSE_AIM\n");
             stage = COARSE_AIM;
+            printf("AUTO switch to DRIVE_AIM_WINCH\n");
+            stage = DRIVE_AIM_WINCH;
+            static int logCap;
+            if(logCap % 20 == 0)
+            {
+                log << "Auto-State is now DRIVE_AIM_WINCH\n";
+                log.flush();
+            }
+            logCap++;
             return;
         case COARSE_AIM:
             if(tilt(HIGHGOAL_AUTOANGLE))
             {
                 printf("AUTO switch to FINE_AIM\n");
                 stage = FINE_AIM;
+                static int logCap;
+                if(logCap % 20 == 0)
+                {
+                    log << "drive:" << driveDone << aimDone << winchDone << "\n";
+                    log.flush();
+                }
+                logCap++;
                 return;
             }
             break;
@@ -126,6 +189,13 @@ void Autonomous::updateHighGoal()
             {
                 printf("AUTO switch to SMART_FIRE\n");
                 stage = SMART_FIRE;
+                static int logCap;
+                if(logCap % 20 == 0)
+                {
+                    log << "Auto-State is now SMART_FIRE";
+                    log.flush();
+                }
+                logCap++;
                 return;
             }
             break;
@@ -158,6 +228,13 @@ void Autonomous::updateHighGoal()
             {
                 printf("AUTO done\n");
                 stage = DONE;
+                static int logCap;
+                if(logCap % 20 == 0)
+                {
+                    log << "Autonomous is now complete, switching to teleop\n";
+                    log.flush();
+                }
+                logCap++;
                 return;
             }
             break;
@@ -178,13 +255,27 @@ void Autonomous::updateBasicDrive()
         case IDLE:
             printf("AUTO switch to BASIC_DRIVE\n");
             stage = BASIC_DRIVE;
+            static int logCap;
+            if(logCap % 20 == 0)
+            {
+                log << "AUTO is in BASIC_DRIVE";
+                log.flush();
+            }
+            logCap++;
             return; // so it doesn't set the previous stage
         case BASIC_DRIVE:
             if(moveForward(DISTANCE))
 //            if(tilt(Shooter::SHOOTING_POSITION))
             {
                 printf("AUTO done\n");
                 stage = DONE;
+                static int logCap;
+                if(logCap % 20 == 0)
+                {
+                    log << "Autonomous is done. Switch to teleop period!\n";
+                    log.flush();
+                }
+                logCap++;
                 return;
             }
             break;
@@ -196,3 +287,9 @@ void Autonomous::updateBasicDrive()
     }
     previousStage = stage;
 }
+
+void Autonomous::testDataLoging()
+{
+    log << "Testing Data Doges";
+    log.flush();
+}

Autonomous.h

@@ -3,6 +3,7 @@
 #include "DriveTrain.h"
 #include "Shooter.h"
 #include <Timer.h>
+#include <fstream>
 
 // the auto methods return true when completed
 
@@ -22,16 +23,18 @@ class Autonomous
     bool timePassed(float time);        //time measured in seconds
     Timer* timer;
     Timer* shotTimer;
-    enum State {COARSE_AIM, FINE_AIM_WAIT, FINE_AIM, IS_HOT, SMART_FIRE, BASIC_DRIVE, IDLE, DONE};
+    enum State {COARSE_AIM, FINE_AIM_WAIT, FINE_AIM, IS_HOT, SMART_FIRE, BASIC_DRIVE, IDLE, DRIVE_AIM_WINCH, DONE};
+    ofstream log;
+    Shooter* autoShooter;
     State stage;
     State previousStage;
     main_robot* robot;
     NetworkTable* table;
 
     void updateHighGoal();
     void updateBasicDrive();
-
     bool getHot();
+    void testDataLoging();
 
     static const double DISTANCE = 151;
     static const double DEGREES_TURN = 25;

DriveTrain.cpp

@@ -3,7 +3,8 @@
 #include "612.h"
 #include "main.h"
 
-const double DriveTrain::SPEED=1.0;
+double DriveTrain::SPEEDR = 1.0;
+double DriveTrain::SPEEDL = 1.0;
 
 // all in feet
 const double DriveTrain::CIRCUMROBOT = 2 * PI * ROBOTRAD;
@@ -37,7 +38,7 @@ void DriveTrain::autoDrive(double distance)
 {
     stopAuto();
     neededDist = distance;
-    TankDrive(SPEED, SPEED);
+    TankDrive(SPEEDL, SPEEDR);
     isMovingL = true;
     isMovingR = true;
     encode->EncoderL->Start();
@@ -52,11 +53,11 @@ void DriveTrain::autoTurn(double degrees)           // any degrees less than zer
     double arcLength = ROBOTRAD * degrees2Radians;  // checks the length of the arc in feet
     neededDist = arcLength;
     if (degrees > 0) {
-        TankDrive(-SPEED, SPEED);
+        TankDrive(-SPEEDL, SPEEDR);
         isTurningL = true;
     }
     if (degrees < 0) {
-        TankDrive(SPEED, -SPEED);
+        TankDrive(SPEEDL, -SPEEDR);
         isTurningR = true;
     }
     hasTurned = false;
@@ -70,15 +71,15 @@ void DriveTrain::teleTurn(Dir direction, double power)
         stopAuto();
     if (direction == RIGHT)
         TankDrive(power,-1*power);
-    else if (direction == LEFT) //jank yo!
+    else if (direction == LEFT) //not jank yo!
         TankDrive(-1*power,power);
 }
 
 void DriveTrain::updateDrive()
 {
     if (isMovingL || isMovingR)
     {
-/*        speedL = SPEED;
+/*      speedL = SPEED;
         speedR = SPEED;
         double varUltraDist = (double)robot->sensors->getUltrasonic();
         if (originUltraDist-varUltraDist >= neededDist)
@@ -92,15 +93,15 @@ void DriveTrain::updateDrive()
         if (speedL = 0.0f && speedR == 0.0f)
             hasDriven = true;
         TankDrive(speedL, speedR);*/
-        speedL = SPEED;           //USING ENCODERS
+        speedL = SPEEDL;           //USING ENCODERS
         if (encode->getLDistance() >= neededDist)
         {
 //            encode->EncoderL->Stop();
 //            encode->EncoderL->Reset();
             isMovingL = false;
             speedL = 0.0f;
         }
-        speedR = SPEED;
+        speedR = SPEEDR;
         if (encode->getLDistance() >= neededDist)
         {
             encode->EncoderL->Stop();
@@ -116,16 +117,27 @@ void DriveTrain::updateDrive()
 
 void DriveTrain::updateTurn()
 {
+	if(encode->numOfRPulses() >= encode->numOfLPulses())
+	{
+		SPEEDL = 1.0f;
+		SPEEDR = (encode->numOfRPulses())/(encode->numOfLPulses());
+	}
+	else
+	{
+		SPEEDR = 1.0f;
+		SPEEDL = (encode->numOfLPulses())/(encode->numOfRPulses());
+	}
+
     if (isTurningL) // NeededDist is positive
     {
-        speedL = SPEED;
+        speedL = SPEEDL;
         if (encode->getLDistance() <= -neededDist)
         {
             encode->EncoderL->Stop();
             encode->EncoderL->Reset();
             speedL = 0.0f;
         }
-        speedR = SPEED;
+        speedR = SPEEDR;
         if (encode->getRDistance() >= neededDist)
         {
             encode->EncoderR->Stop();
@@ -140,14 +152,14 @@ void DriveTrain::updateTurn()
     }
     else if (isTurningR)
     {
-        speedL = SPEED;
+        speedL = SPEEDL;
         if (encode->getLDistance() >= -neededDist)
         {
             encode->EncoderL->Stop();
             encode->EncoderL->Reset();
             speedL = 0.0f;
         }
-        speedR = SPEED;
+        speedR = SPEEDR;
         if (encode->getRDistance() <= neededDist)
         {
             encode->EncoderR->Stop();
@@ -192,3 +204,4 @@ void DriveTrain::stopAuto()
     isTurningR = false;
     hasDriven = false;
 }
+

DriveTrain.h

@@ -37,7 +37,8 @@ class DriveTrain : public RobotDrive
     double originUltraDist;
     float speedL;
     float speedR;
-    static const double SPEED;
+    static double SPEEDL;
+    static double SPEEDR;
     static const double PI = 3.14159265;
     static const double ROBOTRAD = 3.0;
     static const double CIRCUMROBOT;

EncodeDistance.cpp

@@ -45,5 +45,12 @@ double EncodeDistance::convertDistToTick(double distance)
     return (distance/DISTPERPULSE_L);
 }
 
+double EncodeDistance::numOfRPulses()
+{
+	return EncoderR->GetRate();
+}
 
-
+double EncodeDistance::numOfLPulses()
+{
+	return EncoderL->GetRate();
+}

EncodeDistance.h

@@ -16,6 +16,8 @@ class EncodeDistance
     double getAvgDistance();
     double convertTickToDist(double pulse);
     double convertDistToTick(double distance);
+	double numOfRPulses();
+	double numOfLPulses();
     Encoder* EncoderL;
     Encoder* EncoderR;
 private:
@@ -24,6 +26,3 @@ class EncodeDistance
 };
 
 #endif // ENCODEDISTANCE_H
-
-
-