Diff: main.cpp

Revision:

9:0eb7b173d6c3

Parent:

8:d65cba3bfc0e

Child:

10:a3ecedc8d9d7

--- a/main.cpp	Fri Mar 01 20:58:03 2013 +0000
+++ b/main.cpp	Mon Mar 11 14:25:40 2013 +0000
@@ -30,6 +30,7 @@
 void GetSpeeds();
 void SetLeftMotorSpeed(float u);
 void SetRightMotorSpeed(float u);
+void IRChecker();
 
 // Global variables
 float u1 = 0; 
@@ -75,8 +76,8 @@
                 16|-|25 Dir, Left Motor, M1
                 17|-|24 PWM, Left Motor, M1
                 18|-|23 Brake, Right Motor, M2
-                19|-|22 Dir, Right Motor, M2
-                20|-|21 PWM, Right Motor, M2
+Front IR        19|-|22 Dir, Right Motor, M2
+Rear  IR        20|-|21 PWM, Right Motor, M2
 */
 
 // --- IO Port Configuration
@@ -96,7 +97,8 @@
 DigitalOut SpiReset(p9); // Reset for all devices within the slave SPI peripheral in the DE0 FPGA
 DigitalOut SpiStart(p8); // Places SPI interace on the DE0 FPGA into control mode
 InterruptIn Bumper(p10);  // External interrupt pin
-
+AnalogIn IRFront(p19); // Front IR Ranger Input
+AnalogIn IRRear(p20); // Rear IR Ranger Input
 Ticker PeriodicInt;                 
 
 
@@ -483,4 +485,51 @@
     {
         fbSpeedR = -1;
     }
-} 
\ No newline at end of file
+} 
+
+void IRChecker();
+{
+    float IRF, IRR;
+    // Read Sensors
+    IRF = IRFront.read();
+    IRR = IRRear.read();
+    // Voltage Corresponding to nominal distance
+    float Nominal = 3.0;
+    // Variable for turning robots
+    float Turn = 0.1;
+    
+    // Ensure Robot isn't closer than 10cm from wall (reads no voltage)
+    
+    // Compare to nominal voltage and adjust
+    if (IRF > Nominal && IRR > Nominal)
+    {
+        SetR = SetR - Turn;
+        SetL = SetL + Turn;
+        SetRightMotorSpeed(SetR);
+        SetLeftMotorSpeed(SetL);
+    }
+    if (IRF < Nominal && IRR < Nominal
+    {
+        SetR = SetR - Turn;
+        SetL = SetL + Turn;
+        SetRightMotorSpeed(SetR);
+        SetLeftMotorSpeed(SetL);
+    }
+    
+    // Compare rangers to each other
+    if (IRF > IRR) // IRF closer than IRR
+    {
+        SetR = SetR + Turn;
+        SetL = SetL - Turn;
+        SetRightMotorSpeed(SetR);
+        SetLeftMotorSpeed(SetL);
+    }
+    if (IRF < IRR) // IRF further than IRR
+    {
+        SetR = SetR - Turn;
+        SetL = SetL + Turn;
+        SetRightMotorSpeed(SetR);
+        SetLeftMotorSpeed(SetL);
+    }
+   
+}
\ No newline at end of file