Revision 37:23000a47ed2b, committed 2014-03-06

Comitter:

DavidEGrayson

Date:

Thu Mar 06 05:11:46 2014 +0000

Parent:

36:ccb03b734737

Child:

38:5e93a479c244

Commit message:

With a boost 0.2% to handleRight in the Reckoner, this code did very well on the course twice!! Then I ran it the other way and it was more than a foot off :(

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/logger.cpp	Thu Mar 06 05:11:46 2014 +0000
@@ -0,0 +1,42 @@
+#pragma once
+
+#include "logger.h"
+#include "main.h"
+#include "pc_serial.h"
+
+Logger::Logger()
+{
+    entryIndex = 0;   
+}
+
+bool Logger::isFull()
+{
+    return entryIndex >= LOGGER_SIZE;
+}
+
+void Logger::log()
+{
+    if (isFull())
+    {
+        return;   
+    }
+    
+    LogEntry * entry = &entries[entryIndex];
+    entryIndex++;
+    
+    //entry->cos = reckoner.cos >> 16;
+    //entry->sin = reckoner.sin >> 16;
+    entry->x = reckoner.x >> 16;
+    entry->y = reckoner.y >> 16;
+}
+
+void Logger::dump()
+{
+    pc.printf("Log dump start\r\n");
+    for(int32_t i = 0; i < entryIndex; i++)
+    {
+        LogEntry * entry = &entries[i];
+        pc.printf("%d,%d\r\n", entry->x, entry->y);
+    }
+    pc.printf("Log dump end\r\n");
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/logger.h	Thu Mar 06 05:11:46 2014 +0000
@@ -0,0 +1,28 @@
+#pragma once
+
+#include <stdint.h>
+
+#define LOGGER_SIZE 2000
+
+struct LogEntry
+{
+    //int16_t cos;
+    //int16_t sin;
+    int16_t x;
+    int16_t y;   
+};
+
+class Logger
+{
+    public:
+    Logger();
+    void log();
+    void dump();
+    bool isFull();
+    
+    LogEntry entries[LOGGER_SIZE];
+    
+    // The index of the next entry to write to.
+    int32_t entryIndex;
+};
+

--- a/main.cpp	Thu Mar 06 02:39:07 2014 +0000
+++ b/main.cpp	Thu Mar 06 05:11:46 2014 +0000
@@ -15,6 +15,8 @@
 
 Reckoner reckoner;
 LineTracker lineTracker;
+Logger logger;
+Pacer loggerPacer(50000);
 
 const int16_t drivingSpeed = 400;
 
@@ -49,6 +51,8 @@
     //testSensorGlitches();
     //testTurnInPlace();
     //testCloseness();
+    //testLogger();
+    
 
     // Real routines for the contest.
     loadCalibration();
@@ -72,9 +76,29 @@
     //finalSettleIn();
     
     setLeds(1, 1, 1, 1);
-    while(1){}
+    loggerReportLoop();
+}
+
+void loggerService()
+{
+    if (loggerPacer.pace())
+    {
+        logger.log();   
+    }
 }
 
+void loggerReportLoop()
+{
+    while(1)
+    {
+        if(button1DefinitelyPressed())
+        {
+            logger.dump();
+        }
+    }   
+}
+
+
 void loadCalibration()
 {
     /** QTR-3RC **/
@@ -209,7 +233,8 @@
         lineTracker.read();
         lineTracker.updateCalibration();       
         updateReckonerFromEncoders();
-        updateMotorsToDriveStraight();        
+        loggerService();
+        updateMotorsToDriveStraight();
         lineStatus.update(lineTracker.getLineVisible());       
 
         if(lineStatus.getState() == true && lineStatus.getTimeInCurrentStateMicroseconds() > 20000)
@@ -249,6 +274,7 @@
     {
         lineTracker.read();
         updateReckonerFromEncoders();
+        loggerService();
 
         lineStatus.update(lineTracker.getLineVisible());
         
@@ -271,7 +297,8 @@
     while(1)
     {
         updateReckonerFromEncoders();
-        
+        loggerService();
+
         float magn = magnitude();
         
         if (magn < (1<<(14+7)))  
@@ -329,7 +356,8 @@
         led1 = (state == 1);
         
         updateReckonerFromEncoders();
-        
+        loggerService();
+
         float dot = dotProduct();
         int16_t speedModification = -dot * settleModificationStrength;
         if (speedModification > settleModificationStrength)

--- a/main.h	Thu Mar 06 02:39:07 2014 +0000
+++ b/main.h	Thu Mar 06 05:11:46 2014 +0000
@@ -2,6 +2,7 @@
 
 #include "reckoner.h"
 #include "line_tracker.h"
+#include "logger.h"
 
 void loadCalibration();
 
@@ -11,6 +12,7 @@
 void followLineToEnd();
 void driveHomeAlmost();
 void finalSettleIn();
+void __attribute__((noreturn)) loggerReportLoop();
 
 void updateMotorsToFollowLine();
 void updateReckonerFromEncoders();
@@ -18,6 +20,8 @@
 float determinant();
 float dotProduct();
 float magnitude();
+void loggerService();
 
 extern Reckoner reckoner;
 extern LineTracker lineTracker;
+extern Logger logger;
\ No newline at end of file

--- a/reckoner.cpp	Thu Mar 06 02:39:07 2014 +0000
+++ b/reckoner.cpp	Thu Mar 06 05:11:46 2014 +0000
@@ -153,7 +153,9 @@
 
 void Reckoner::handleRight()
 {
-    handleTurnRadians(-DA);
+    // DA = 4790484
+    // 0.2% boost
+    handleTurnRadians(-4800065);
 }
 
 void Reckoner::handleLeft()
@@ -165,6 +167,8 @@
 {
     int32_t dc = -((int64_t)sin * radians) >> LOG_UNIT_MAGNITUDE;
     int32_t ds = ((int64_t)cos * radians) >> LOG_UNIT_MAGNITUDE;
+    dc = -((int64_t)(sin+ds/2) * radians) >> LOG_UNIT_MAGNITUDE;
+    ds = ((int64_t)(cos+dc/2) * radians) >> LOG_UNIT_MAGNITUDE;
     cos += dc;
     sin += ds;
 }
\ No newline at end of file

--- a/test.cpp	Thu Mar 06 02:39:07 2014 +0000
+++ b/test.cpp	Thu Mar 06 05:11:46 2014 +0000
@@ -16,6 +16,20 @@
 void __attribute__((noreturn)) infiniteReckonerReportLoop();
 void printBar(const char * name, uint16_t adcResult);
 
+void testLogger()
+{
+    led1 = 1;
+    while(!button1DefinitelyPressed())
+    {
+        led3 = logger.isFull();
+    
+        updateReckonerFromEncoders();
+        loggerService();
+    }
+    led2 = 1;
+    loggerReportLoop();
+}
+
 void testCloseness()
 {
     led1 = 1;

--- a/test.h	Thu Mar 06 02:39:07 2014 +0000
+++ b/test.h	Thu Mar 06 05:11:46 2014 +0000
@@ -12,4 +12,5 @@
 void __attribute__((noreturn)) testAnalog();
 void __attribute__((noreturn)) testSensorGlitches();
 void __attribute__((noreturn)) testTurnInPlace();
-void __attribute__((noreturn)) testCloseness();
\ No newline at end of file
+void __attribute__((noreturn)) testCloseness();
+void __attribute__((noreturn)) testLogger();
\ No newline at end of file