Revision 6:32d9b855b90f, committed 2015-11-18

Comitter:

intgsull

Date:

Wed Nov 18 06:49:06 2015 +0000

Parent:

5:f9837617817b

Child:

7:f1bceb2bab70

Commit message:

tryna fix irPID

Changed in this revision

--- a/AVEncoder.lib	Sat Nov 14 01:12:47 2015 +0000
+++ b/AVEncoder.lib	Wed Nov 18 06:49:06 2015 +0000
@@ -1,1 +1,1 @@
-http://developer.mbed.org/users/aravindsv/code/AVEncoder/#a32ab75adcad
+http://developer.mbed.org/users/aravindsv/code/AVEncoder/#e0e5901955cd

--- a/main.cpp	Sat Nov 14 01:12:47 2015 +0000
+++ b/main.cpp	Wed Nov 18 06:49:06 2015 +0000
@@ -1,6 +1,7 @@
 //Micromouse code
 #include "mbed.h"
 #include "AVEncoder.h"
+#include "analogin_api.h"
 
 // set things
 Serial pc(SERIAL_TX, SERIAL_RX);
@@ -50,24 +51,23 @@
 volatile float gyro_prevError = 0;
 
 volatile float line_accumulator = 0;
-volatile float line_decayFactor = 1.5;
+volatile float line_decayFactor = 1;
 volatile float enco_accumulator = 0;
-volatile float enco_decayFactor = 1.5;
+volatile float enco_decayFactor = 1;
 volatile float gyro_accumulator = 0;
-volatile float gyro_decayFactor = 1.5;
+volatile float gyro_decayFactor = 1;
 
 volatile float left_speed = 0;
 volatile float right_speed = 0;
 
-volatile unsigned long l_pulses = 0;
-volatile unsigned long r_pulses = 0;
-
 // pid constants. these need to be tuned. but i set them as a default val for now.
 // line refers to the translational speed. 
 // enco and gyro will be used primarily for angular speed. 
 // (we can change the names later, 
 // i added line in after i realized that i already had the angular code)
-const float line_propo = 1;
+const int   max_speed = 6; // max speed is 6 encoder pulses per ms.
+
+const float line_propo = 0;
 const float line_integ = 0;
 const float line_deriv = 0;
 
@@ -92,7 +92,7 @@
     TURNING, 
     UNKNOWN
 } STATE;
-STATE mouse_state;
+volatile STATE mouse_state;
 
 // helper functions
 void reset();
@@ -101,15 +101,87 @@
 void moveForward();
 void turn();
 
-// interrupt stuff. 
-void incLENC()
-{
-    l_pulses++;
+
+//irPID function to not crash into stuff 
+
+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 leftDistance = 0;
+volatile float rightDistance= 0;
+void irPID()       
+{    
+ eLS = 1;
+ eRS = 1;  
+ if(rLS > leftWall && rRS > rightWall)//walls on both sides
+    {  
+        leftDistance = rLS;
+        rightDistance = rRS;
+        irError = rightDistance; //– leftDistance – irOffset;
+        irError -= leftDistance;
+        irError -= irOffset;
+        
+        irErrorD = irError;
+        irErrorD -= oldirError;//(irError – oldirError);
+        
+        irErrorI += irError;
+    }        
+    else if(rLS > leftWall) //just left wall
+    {
+        leftDistance = rLS;
+        irError = lirOffset;//(2 * (lirOffset – leftDistance));
+        
+        irErrorD = (irError – oldirError);
+    irErrorI += irError;
+    }
+    else if(rRS > rightWall)//just right wall
+    {
+        rightDistance = rRS();
+        irError = (2 * (rightDistance – rirOffset));
+        irErrorD = (irError – oldirError);
+    irErrorI += irError;
+    }
+    else if(rLS < leftWall && rRS < 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; 
+    eLS = 0;
+    eRS = 0;   
 }
 
-void incRENC()
+const float frontWall = 0; //need to calibrate this threshold to a value where mouse can stop in time
+//something like this may be useful for the very temporal present
+//doesn't account for current speed/trajectory atm
+void dontDriveStraightIntoAWall()
 {
-    r_pulses++;
+    eRF = 1;
+    eLF = 1;
+    if(rRF > frontWall || rLF > frontWall)
+    {
+        eRF = 0;
+        eLF = 0;
+        mouse_state = STOPPED;
+        stop();
+        return;
+    }
 }
 
 // PID Control
@@ -117,21 +189,23 @@
 // we do have to calibrate constants though. 
 void systick()
 {
-    pc.printf("systick ran\r\n");
+    //pc.printf("systick ran\r\n");
     if ( mouse_state == STOPPED || mouse_state == UNKNOWN )
     {
         // do nothing? 
         // reset?
-        reset();
+        //reset();
         offsetCalc();
         return;
     }
-    pc.printf("systick ran while state is FORWARD \r\n");
+    //pc.printf("systick ran while state is FORWARD \r\n");
     
     int dt = timer.read_us(); // should be around 1 ms.
     timer.reset();
     
-    float line_error = line_speed * dt - 0.5 * ( l_enco.getPulses() - r_enco.getPulses());
+    //printf("The number of left motor pulses is: %d, right pulses: %d\r\n", l_enco.getPulses(), r_enco.getPulses());
+    
+    float line_error = line_speed - 0.5 * ( l_enco.getPulses() + r_enco.getPulses());
     int enco_error = l_enco.getPulses() - r_enco.getPulses();
     float gyro_error = _gyro.read() - gyro_offset;
     
@@ -152,17 +226,17 @@
     float gyro_pid = 0;
     gyro_pid += gyro_propo * gyro_error;
     gyro_pid += gyro_integ * gyro_accumulator;
-    gyro_pid += gyro_deriv * (gyro_error - gyro_prevError)/dt;
+    gyro_pid += gyro_deriv *(gyro_error - gyro_prevError)/dt;
         
     // forward moving pid control. 
     if ( mouse_state == FORWARD )
     {
         float x_error = line_pid;
-        float w_error = spin_enco_weight * enco_pid + spin_gyro_weight * gyro_pid;
-        left_speed += x_error + w_error;
-        right_speed += x_error - w_error;
+        float w_error = 0; //spin_enco_weight * enco_pid + spin_gyro_weight * gyro_pid;
+        left_speed += (x_error - w_error);
+        right_speed += (x_error + w_error);   //swapped
         
-        pc.printf("left_speed: %f, right_speed: %f\r\n", left_speed, right_speed);
+        //pc.printf("left_speed: %f, right_speed: %f\r\n", left_speed, right_speed);
         
         moveForward();
         // offsetCalc();
@@ -188,8 +262,12 @@
 void setup()
 {
     pc.printf("Hello World\r\n");
+    pc.printf("left_speed: %f, right_speed: %f\r\n", left_speed, right_speed);
+    pc.printf("The number of left motor pulses is: %d, right pulses: %d\r\n", l_enco.getPulses(), r_enco.getPulses());
     mouse_state = STOPPED;
     
+    reset(); 
+    
     eRS = 0;
     eRF = 0;
     eLS = 0;
@@ -228,12 +306,19 @@
 int main()
 {
     setup();
-    mouse_state = FORWARD;
+    pc.printf("left_speed: %f, right_speed: %f\r\n", left_speed, right_speed);
+    
+    wait(2);
     
-    wait(1.5);
+    mouse_state = FORWARD;
+    for ( int i = 0; i < 10; i++ )
+    {
+        pc.printf("left_speed: %f, right_speed: %f\r\n", left_speed, right_speed);
+        pc.printf("The number of left motor pulses is: %d, right pulses: %d\r\n", l_enco.getPulses(), r_enco.getPulses());
+    }
     stop();
     
-    pc.printf("DONE\r\n");
+    //pc.printf("The number of left motor pulses is: %d, right pulses: %d\r\n", l_enco.getPulses(), r_enco.getPulses());
     
     //while(1)
 //    {
@@ -247,28 +332,28 @@
 // movement functions.
 void moveForward()
 {
-    mouse_state = FORWARD;
+    //mouse_state = FORWARD;
     
     if ( left_speed > 0 ) // which should be always.
     {
-        motor1_forward = left_speed;
+        motor1_forward = left_speed / max_speed;
         motor1_reverse = 0;
     }
     else
     {
         motor1_forward = 0;
-        motor1_reverse = -left_speed;
+        motor1_reverse = -left_speed / max_speed;
     }
     
     if ( right_speed > 0 ) // which again should be always.
     {
-        motor2_forward = right_speed;
+        motor2_forward = right_speed / max_speed;
         motor2_reverse = 0;
     }
     else
     {
         motor2_forward = 0;
-        motor2_reverse = -right_speed;
+        motor2_reverse = -right_speed / max_speed;
     }
 }