Diff: main.cpp

Revision:

4:112f3d35bd2d

Parent:

3:40333f38771d

Child:

5:f704940c9c7e

--- a/main.cpp	Tue Nov 24 05:24:55 2015 +0000
+++ b/main.cpp	Tue Dec 01 19:38:27 2015 +0000
@@ -47,29 +47,29 @@
 volatile float line_accumulator = 0;
 volatile float line_decayFactor = 1;
 volatile float enco_accumulator = 0;
-volatile float enco_decayFactor = 2;
+volatile float enco_decayFactor = 1.6;
 volatile float gyro_accumulator = 0;
 volatile float gyro_decayFactor = 1;
 
-volatile float set_speed = 1;
-volatile float left_speed = 2;
-volatile float right_speed = 2;
+volatile float set_speed = .75;
+volatile float left_speed = .75;
+volatile float right_speed = .75;
 
-const float left_max_speed = 5.5; // max speed is 6 encoder pulses per ms.
-const float right_max_speed = 5.7; 
+const float left_max_speed = 5; // max speed is 6 encoder pulses per ms.
+const float right_max_speed = 5; 
 
 const float gyro_propo = 6.5;
 const float gyro_integ = 0;
 const float gyro_deriv = 10;
 
-const float enco_propo = 1;
-const float enco_integ = 3;//1;
-const float enco_deriv = 40;//.0002;
+const float enco_propo = 10;
+const float enco_integ = 15;//1;
+const float enco_deriv = 2000;//.0002;
 
 const float spin_enco_weight = 1;
 const float spin_gyro_weight = 1 - spin_enco_weight;
 
-const float frontWall = 0.25; //need to calibrate this threshold to a value where mouse can stop in time
+const float frontWall = 0.2; //need to calibrate this threshold to a value where mouse can stop in time
 //something like this may be useful 
 
 volatile float enco_error;
@@ -78,6 +78,69 @@
 volatile float gyro_pid;
 volatile float w_error;
 
+////////////////////////////////////ir PID constants////////////////////////////////////////////////
+const float leftWall= 0;
+const float rightWall= 0;   //we need to find the threshold value for when 
+//left or right walls are present 
+const float irOffset= 0; // difference between right and left ir sensors when
+//mouse is in the middle
+const float lirOffset= 0; //middle value for when there is only a wall on one 
+const float rirOffset= 0; //side of the mouse (left and right)
+volatile float irError = 0;
+volatile float irErrorD = 0;
+volatile float irErrorI = 0;
+volatile float oldirError = 0;
+volatile float totalirError= 0; //errors
+const float irKp = 0;
+const float irKd = 0; 
+const float irKi = 0; //constants
+volatile float leftD;
+volatile float rightD;
+void irPID()       
+{    
+ eLS = 1;
+ leftD = rLS.read();
+ eLS = 0;
+ eRS = 1;  
+ rightD = rRS.read();
+ eRS = 0;
+ if(leftD > leftWall && rightD > rightWall)//walls on both sides
+    {  
+        irError = rightD-leftD-irOffset; 
+        
+        irErrorD = irError-oldirError;
+        
+        irErrorI += irError;
+    }        
+    else if(leftD > leftWall) //just left wall
+    {
+    
+        irError = 2*(lirOffset-leftD);
+        
+        irErrorD=irError-oldirError;
+        
+        irErrorI += irError;
+    }
+    else if(rightD > rightWall)//just right wall
+    {
+        irError=2*(rightD-rirOffset); 
+        
+        irError = irError-oldirError;
+    
+        irErrorI += irError;
+    }
+    else if(leftD < leftWall && rightD < rightWall)//no walls!! Use encoder PID
+    {
+        irError = 0;
+        irErrorD = 0;
+        irErrorI += irError; 
+    }
+    totalirError =  irError*irKp + irErrorD*irKd + irErrorI*irKi;
+    oldirError = irError;
+    left_speed -= totalirError;
+    right_speed += totalirError;  
+}
+
 // this is just so that we can maintain what state our mouse is in. 
 // currently this has no real use, but it may in the future. 
 // or we could just remove this entirely. 
@@ -184,6 +247,7 @@
 }
 
 
+
 void setup()
 {
     pc.printf("Hello World\r\n");