Revision 33:58a0ab6e9ad2, committed 2014-03-05

Comitter:

DavidEGrayson

Date:

Wed Mar 05 02:50:09 2014 +0000

Parent:

32:83a13b06093c

Child:

34:6c84680d823a

Commit message:

Bunch of stuff. Then reduced drivingSpeed to 400.

Changed in this revision

--- a/main.cpp	Tue Mar 04 04:32:51 2014 +0000
+++ b/main.cpp	Wed Mar 05 02:50:09 2014 +0000
@@ -16,7 +16,7 @@
 Reckoner reckoner;
 LineTracker lineTracker;
 
-const int16_t drivingSpeed = 300;
+const int16_t drivingSpeed = 400;
 
 void setLeds(bool v1, bool v2, bool v3, bool v4)
 {
@@ -41,30 +41,36 @@
     //testLineSensors();
     //testReckoner();
     //testButtons();
-    //testDriveHome();
+    testDriveHome();
     //testFinalSettleIn();
     //testCalibrate();
-    testLineFollowing();
+    //testLineFollowing();
     //testAnalog();
     //testSensorGlitches();
+    //testTurnInPlace();
+    //testCloseness();
 
     // Real routines for the contest.
     loadCalibration();
     
     setLeds(1, 0, 0, 0);
     waitForSignalToStart();
-    setLeds(0, 1, 0, 0);
     
+    setLeds(0, 1, 0, 0);    
     findLine();
     
     //setLeds(1, 1, 0, 0);
     //turnRightToFindLine();
+    
     setLeds(0, 0, 1, 0);
     followLineToEnd();
+    
     setLeds(1, 0, 1, 0);
     driveHomeAlmost();
-    setLeds(0, 1, 1, 0);
-    finalSettleIn();
+    
+    //setLeds(0, 1, 1, 0);
+    //finalSettleIn();
+    
     setLeds(1, 1, 1, 1);
     while(1){}
 }
@@ -178,7 +184,7 @@
 
 void updateMotorsToFollowLine()
 {
-    const int followLineStrength = 300;
+    const int followLineStrength = drivingSpeed;
 
     int16_t speedLeft = drivingSpeed;
     int16_t speedRight = drivingSpeed;
@@ -206,13 +212,14 @@
         updateMotorsToDriveStraight();        
         lineStatus.update(lineTracker.getLineVisible());       
 
-        if(lineStatus.getState() == true && lineStatus.getTimeInCurrentStateMicroseconds() > 100000)
+        if(lineStatus.getState() == true && lineStatus.getTimeInCurrentStateMicroseconds() > 20000)
         {
             break;
         }
     }
 }
 
+/**
 void turnRightToFindLine()
 {   
     while(1)
@@ -228,7 +235,7 @@
         
         motorsSpeedSet(300, 100);        
     }
-}
+}**/
 
 void followLineToEnd()
 {
@@ -267,9 +274,9 @@
         
         float magn = magnitude();
         
-        if (magn < (1<<17))  
+        if (magn < (1<<(14+7)))  
         {
-            // We are within 8 encoder ticks, so go to the next step.
+            // We are within 128 encoder ticks, so go to the next step.
             break;
         }
         
@@ -277,9 +284,9 @@
         
         int16_t speedLeft = drivingSpeed;
         int16_t speedRight = drivingSpeed;
-        if (magn < (1<<20)) // Within 64 encoder ticks of the origin, so slow down.
+        if (magn < (1<<(14+9))) // Within 512 encoder ticks of the origin, so slow down.
         {
-            int16_t reduction = (1 - magn/(1<<20)) * drivingSpeed;
+            int16_t reduction = (1 - magn/(1<<(14+8))) * (drivingSpeed/2);
             speedLeft = reduceSpeed(speedLeft, reduction);
             speedRight = reduceSpeed(speedRight, reduction);
         }

--- a/main.h	Tue Mar 04 04:32:51 2014 +0000
+++ b/main.h	Wed Mar 05 02:50:09 2014 +0000
@@ -14,8 +14,10 @@
 
 void updateMotorsToFollowLine();
 void updateReckonerFromEncoders();
+void setLeds(bool v1, bool v2, bool v3, bool v4);
 float determinant();
 float dotProduct();
+float magnitude();
 
 extern Reckoner reckoner;
 extern LineTracker lineTracker;

--- a/test.cpp	Tue Mar 04 04:32:51 2014 +0000
+++ b/test.cpp	Wed Mar 05 02:50:09 2014 +0000
@@ -16,56 +16,75 @@
 void __attribute__((noreturn)) infiniteReckonerReportLoop();
 void printBar(const char * name, uint16_t adcResult);
 
-uint16_t readPin18()
+void testCloseness()
 {
-    // Set PCADC bit in PCONP register.  (Table 46).
-    LPC_SC->PCONP |= (1 << 12);
-    
-    // Select PCLK_ADC in PCLKSEL0 register. (Table 40, Table 531).
-    LPC_SC->PCLKSEL0 |= (3 << 24);  // PCLK for ADC = CCLK/8 
-    
-    // Enable ADC0 through PINSEL registers.  (Section 8.5).
-    LPC_PINCON->PINSEL1 = (LPC_PINCON->PINSEL1 & ~(3 << 20)) | (1 << 20);
-    
-    // Pin 18: P0.26/AD0.3/AOUT/RXD3
-    
-    // 7:0   Bitmap to select what channel to use
-    // 15:8  Select clock.
-    // 16    BURST = 0
-    // 20:17 Reserved
-    // 21    PDN = 1, A/D converter is operational
-    // 26:24 START = 001 to start conversion now
-    LPC_ADC->ADCR = (1 << 3) | (0xFF << 8) | (1 << 21);
-    LPC_ADC->ADCR |=  (1 << 24);  
-    
-    while(!(LPC_ADC->ADGDR >> 31 & 1))  // while not done
+    led1 = 1;
+    while(1)
     {
+        updateReckonerFromEncoders();
+        float magn = magnitude();
+        
+        led3 = (magn < (1<<(14+7)));
+        led4 = (magn < (1<<(14+9)));
     }
-    //return 2;
-    return LPC_ADC->ADGDR & 0xFFFF;
+}
+
+void showOrientationWithLeds34()
+{
+    led3 = reckoner.cos > 0;
+    led4 = reckoner.sin > 0;
 }
 
-uint16_t readP10()
+void testTurnInPlace()
 {
-    DigitalInOut pin(p10);
-    pin.mode(PullNone);
-    pin.output();
-    pin = 1;
-    wait_us(20);
-    uint16_t value = 1000;
+    led1 = 1;
+    while(!button1DefinitelyPressed())
+    {
+        updateReckonerFromEncoders();
+        showOrientationWithLeds34();
+    }
+    led2 = 1;
+    
+    Pacer motorUpdatePacer(10000);
     Timer timer;
     timer.start();
-    pin.input();
-    while(timer.read_us() < 1000)
+    motorsSpeedSet(-300, 300);
+    while(timer.read_ms() < 4000)
+    {
+        updateReckonerFromEncoders();
+        showOrientationWithLeds34();
+    }
+    timer.reset();
+    
+    float integral = 0;
+    while (timer.read_ms() < 4000)
     {
-        if(pin.read() == 0)
+        if (motorUpdatePacer.pace())
         {
-            return timer.read_us();   
+            int16_t rotationSpeed;
+            float s = (float)reckoner.sin / (1 << 30);
+            integral += s;
+            rotationSpeed = -(s * 2400 + integral * 20);
+            
+            if (rotationSpeed > 450)
+            {
+                rotationSpeed = 450; 
+            }
+            if (rotationSpeed < -450)
+            {
+                rotationSpeed = -450; 
+            }
+            
+            int16_t speedLeft = -rotationSpeed;
+            int16_t speedRight = rotationSpeed;
+            motorsSpeedSet(speedLeft, speedRight);
         }
     }
-    return value;
+    
+    infiniteReckonerReportLoop();      
 }
 
+
 void testSensorGlitches()
 {
     AnalogIn testInput(p18);
@@ -98,7 +117,10 @@
             }
         }
         **/
-        reading = readP10();
+
+        uint16_t values[LINE_SENSOR_COUNT];        
+        readSensors(values);
+        reading = values[0];
         
         if(reading > 100)
         {
@@ -208,8 +230,14 @@
     {
         updateReckonerFromEncoders();
     }
+    
+    setLeds(1, 0, 1, 0);
     driveHomeAlmost();
-    finalSettleIn();
+    
+    //setLeds(0, 1, 1, 0);
+    //finalSettleIn();
+    
+    setLeds(1, 1, 1, 1);
     infiniteReckonerReportLoop();     
 }
 
@@ -250,10 +278,10 @@
     while(1)
     {
         updateReckonerFromEncoders();
-        led1 = (reckoner.cos > 0);
-        led2 = (reckoner.sin > 0);
-        led3 = (reckoner.x > 0);
-        led4 = (reckoner.y > 0);
+        
+        led1 = (reckoner.x > 0);
+        led2 = (reckoner.y > 0);
+        showOrientationWithLeds34();
         
         if (reportPacer.pace())
         {
@@ -421,13 +449,12 @@
     Pacer reportPacer(200000);
     while(1)
     {
+        showOrientationWithLeds34();
         if(reportPacer.pace())
         {
-            led4 = 1;
             pc.printf("%11d %11d %11d %11d | %11f %11f\r\n",
               reckoner.cos, reckoner.sin, reckoner.x, reckoner.y,
               determinant(), dotProduct());
-            led4 = 0;
        }
     }
    

--- a/test.h	Tue Mar 04 04:32:51 2014 +0000
+++ b/test.h	Wed Mar 05 02:50:09 2014 +0000
@@ -11,3 +11,5 @@
 void __attribute__((noreturn)) testLineFollowing();
 void __attribute__((noreturn)) testAnalog();
 void __attribute__((noreturn)) testSensorGlitches();
+void __attribute__((noreturn)) testTurnInPlace();
+void __attribute__((noreturn)) testCloseness();
\ No newline at end of file