Revision 18:4b3ad79d1068, committed 2013-03-22

Comitter:

LtBarbershop

Date:

Fri Mar 22 19:57:55 2013 +0000

Parent:

17:bc13550f673b

Child:

19:b9c4952f4acc

Commit message:

tried some stuff, no too much going on

Changed in this revision

--- a/main.cpp	Wed Mar 20 01:42:03 2013 +0000
+++ b/main.cpp	Fri Mar 22 19:57:55 2013 +0000
@@ -61,11 +61,12 @@
 float DR = 0;
 float TSpeedL = 0;
 float TSpeedR = 0;
-float Turn = 0.01;
+float Turn = 0.03;
 float aeW = 0;
 float eW = 0;
 float uW = 0; 
 float prevuW = 0;
+float Nominal = 25;
 
 
 Mutex Var_Lock;
@@ -117,11 +118,11 @@
 SPI DE0(p5, p6, p7); // (mosi, miso, sclk) DE0 is the SPI channel with the DE0 FPGA 
 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 EmerStop(p10);  // External interrupt pin
+//InterruptIn EmerStop(p10);  // External interrupt pin
 AnalogIn IRFront(p19); // Front IR Ranger Input
 AnalogIn IRRear(p20); // Rear IR Ranger Input
 Ticker PeriodicInt;                 
-
+DigitalIn EmergencyStop(p10);
 
 // ******** Main Thread ********
 int main() 
@@ -144,7 +145,12 @@
             if (c == 'q')
             {
                 action = 0;
-                Ramp(0, 1000, 0);
+                // erase errors
+                aeW = 0;
+                eW = 0;
+                aeL = 0;
+                aeR = 0;
+                Ramp(0, 800, 0);
                 DisplayMenu();
                 continue;
             }
@@ -161,6 +167,7 @@
                     case 'w':
                         BtS.printf("\n\r Welcome to Wall Following, use w-s to control robot speed");
                         BtS.printf("\n\r Use a-d to increase/decrease turning radius");
+                        BtS.printf("\n\r Use < and > to increase/decrease wall distance");
                         userSetL = 0.1;
                         userSetR = 0.1;
                         TSpeedL = 0.1;
@@ -174,9 +181,6 @@
                         action = 4;
                         
                         break;
-                    case 'q':
-                        action = 0;
-                        break;
                     default:
                         BtS.printf("\n\r Command not recognized \n\r");
                         action = 0;
@@ -228,6 +232,12 @@
                         break;
                     case('d'):
                         Turn = Turn - 0.005;
+                        break;
+                    case(','):
+                        Nominal = Nominal - 5;
+                        break;
+                    case('.'):
+                        Nominal = Nominal + 5;
                         break;               
                 }
             }
@@ -260,13 +270,13 @@
         {
             // Wall Following
             //Var_Lock.lock();
-            //BtS.printf("IR F: %f cm R: %f cm \n\r", DF, DR);
-//            BtS.printf("Wall Error: %f \n\r", eW);
-//            BtS.printf("Acc Error: %f \n\r", aeW);
-            //BtS.printf("Diff. Setpoint: %f \n\r", uW);
-//            BtS.printf("Setpoint L: %f R: %f \n\n\r", userSetL, userSetR);
+            BtS.printf("IR F: %f cm R: %f cm \n\r", DF, DR);
+            BtS.printf("Wall Error: %f \n\r", eW);
+            BtS.printf("Acc Error: %f \n\r", aeW);
+            BtS.printf("Diff. Setpoint: %f \n\r", uW);
+            BtS.printf("Setpoint L: %f R: %f \n\n\r", userSetL, userSetR);
             //Var_Lock.unlock();
-            //Thread::wait(1000);
+            Thread::wait(1000);
         }
         
         if (action == 3)
@@ -341,6 +351,16 @@
     
     while (1) 
     {
+        // check for emergency stop
+        if (EmergencyStop == 1)
+        {
+        userSetL = 0;
+        userSetR = 0;
+        SetLeftMotorSpeed(userSetL);
+        SetRightMotorSpeed(userSetR);
+        BtS.printf("\n\rEmergency Stop!!\n\r");
+        }
+        
         osSignalWait(SignalPi, osWaitForever); 
         led2= !led2; // Alive status
         
@@ -476,7 +496,7 @@
     led3=0;
     led4=0; 
     
-    EmerStop.rise(&ExtCollisionISR); // Atach the address of the interrupt handler to the rising edge of Bumper
+    //EmerStop.rise(&ExtCollisionISR); // Atach the address of the interrupt handler to the rising edge of Bumper
     
     // Start execution of the Threads
     PiControl = osThreadCreate(osThread(PiControlThread), NULL);
@@ -636,11 +656,11 @@
     float bF = -0.1721; // -0.2608
     float aR = 22.6021; // 34.2456
     float bR = -0.0376; // -0.0569
-    float Nominal = 25;
-    float Kpos = 0.02;
-    float Kor = 0.01;
-    float KpW = 0.1;
-    float KiW = 0.4;
+    float Nominal = 35;
+    float Kpos = 0.1;
+    float Kor = 0.0;
+    float KpW = 0.01;
+    float KiW = 0.02;
     
     // Read Sensors
     IRF1 = 3.3*IRFront.read();
@@ -655,6 +675,8 @@
     // Calculate distance based on voltage
     prevDF = DF;
     prevDR = DR;
+    
+    
     DF = aF/(IRF+bF);
     DR = aR/(IRR+bR);
     
@@ -671,20 +693,20 @@
     eW = Kpos*(Nominal - (DF + DR)/2) + Kor*(DR - DF);
     
     // accumulate error
-    if (prevuW != 0.025 || prevuW != -0.025)
+    if (prevuW < Turn && prevuW > -Turn)
     {
         aeW = aeW + eW;
     }
     
     uW = KpW*eW + KiW*aeW;
     
-    if (uW > 0.05)
+    if (uW > Turn)
     {
-        uW = 0.05;
+        uW = Turn;
     }
-    else if (uW < -0.05)
+    else if (uW < -Turn)
     {
-        uW = -0.05;
+        uW = -Turn;
     }
     
     // set differential speeds
@@ -700,7 +722,7 @@
     float bF = -0.1721; // -0.2608
     float aR = 22.6021; // 34.2456
     float bR = -0.0376; // -0.0569
-    float Nominal = 25;
+    // float Nominal = 25;
     
     // Read Sensors
     IRF = 3.3*IRFront.read();