Diff: test.cpp

Revision:

33:58a0ab6e9ad2

Parent:

32:83a13b06093c

Child:

37:23000a47ed2b

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