Diff: main.cpp

Revision:

6:dfbcdc63d4f8

Parent:

5:c673430c8a32

diff -r c673430c8a32 -r dfbcdc63d4f8 main.cpp
--- a/main.cpp	Sun Jan 10 19:12:08 2016 +0000
+++ b/main.cpp	Tue Feb 16 22:45:21 2016 +0000
@@ -17,7 +17,6 @@
 //comment out following line for normal operation
 //#define TUNE_PID 1
 
-    
 //state variables for ISR
 volatile bool run = false;
 BusOut start_stop_led(D6, D7);
@@ -32,6 +31,12 @@
     start_stop_led = (start_stop_led ^ 3);
 }
 
+//Function for reading QTR-RC infrared sensor from Pololu
+uint8_t get_ir_bus(BusInOut &ir_bus, uint16_t *ir_vals, uint16_t &sample_cnt);
+
+//constants used with Pololu sensor
+const uint16_t MAX_SAMPLE_TIME = 1000;
+const uint8_t NUM_SENSORS = 8;
 
 int main(void)
 {
@@ -43,10 +48,12 @@
     start_stop_led = 2; // D7 on D6 off = red
     
     //Connect IR sensor to port 4
-    BusIn ir_bus(P4_0, P4_1, P4_2, P4_3, P4_4, P4_5, P4_6, P4_7);
-    DigitalOut ir_bus_enable(D3, 1); //active high enable
+    BusInOut ir_bus(P4_0, P4_1, P4_2, P4_3, P4_4, P4_5, P4_6, P4_7);
     
-    //raw sensor data
+    uint16_t ir_vals[8]; 
+    uint16_t samples = 0;
+    
+    //binary sensor data
     uint8_t ir_val = 0;
     
     //used to measure dt
@@ -68,7 +75,7 @@
     const int32_t MAX_DUTY_CYCLE = 80;
     const int32_t MIN_DUTY_CYCLE = -80;
     
-    const int32_t TARGET_DUTY_CYCLE = 37;
+    const int32_t TARGET_DUTY_CYCLE = 75;  //was 37
     
     
     /***************************************************
@@ -89,9 +96,9 @@
     //Set PID terms to 0 if not used/needed
     //For values below Kc = 10 and Pc was measured at 0.33 calculated starting points given in comments below
     //Ended up decreasing integral term, increasing derivative term and small decrease in proportional term.
-    int32_t kp = 4; //6
-    int32_t ki = 3; //.0576, divide by 1000 later, 7
-    int32_t kd = 250; //156.25, 500 
+    int32_t kp = 5; //was 4
+    int32_t ki = 0; //.0576, divide by 1000 later, was 2
+    int32_t kd = 0; //156.25, was 250
     
     //initialize vars
     int32_t current_error = 0;
@@ -142,11 +149,24 @@
     #endif//TUNE_PID
     
     
-    //loop time is ~1.6ms
+    //loop time is ~2.86ms
     for(;;)
     {   
         //wait for start_stop button press
-        while(!run);
+        while(!run)
+        {
+            //get raw ir sensor data
+            ir_val = ~(get_ir_bus(ir_bus, ir_vals, samples)); 
+            
+            printf("\nir_state = 0x%2x \t samples = %d\n\n", ir_val, samples);
+            for(uint8_t idx = 0; idx < 8; idx++)
+            {
+                printf("%.4d \t", ir_vals[idx]);
+            }
+            printf("\n\n");
+            
+            wait(1.0);
+        }
         
         //mode is set to forward, but duty cycle is still 0
         motor_shield.set_operating_mode(l_motor_driver, Max14871_Shield::FORWARD);
@@ -158,7 +178,7 @@
             loop_pulse = !loop_pulse;
             
             //get raw ir sensor data
-            ir_val = ~(ir_bus.read());
+            ir_val = ~(get_ir_bus(ir_bus, ir_vals, samples)); 
             
             //scale feedback
             switch(ir_val)
@@ -262,7 +282,7 @@
             previous_error = current_error;
             
             //get new duty cycle for right motor
-            r_duty_cycle = (TARGET_DUTY_CYCLE - (kp*current_error + ((ki*integral)/1000) + kd*derivative));
+            r_duty_cycle = (TARGET_DUTY_CYCLE + (kp*current_error + ((ki*integral)/1000) + kd*derivative));
             
             //clamp to limits
             if((r_duty_cycle > MAX_DUTY_CYCLE) || (r_duty_cycle < MIN_DUTY_CYCLE))
@@ -279,7 +299,7 @@
        
     
             //get new duty cycle for left motor  
-            l_duty_cycle  = (TARGET_DUTY_CYCLE + (kp*current_error + ((ki*integral)/1000) + kd*derivative));
+            l_duty_cycle  = (TARGET_DUTY_CYCLE - (kp*current_error + ((ki*integral)/1000) + kd*derivative));
             
             //clamp to limits
             if((l_duty_cycle > MAX_DUTY_CYCLE) || (l_duty_cycle < MIN_DUTY_CYCLE))
@@ -335,3 +355,62 @@
         derivative = 0;
     }
 }
+
+
+//Function for reading QTR-RC infrared sensor from Pololu
+uint8_t get_ir_bus(BusInOut &ir_bus, uint16_t *ir_vals, uint16_t &sample_cnt)
+{
+    Timer ir_timer; 
+    uint8_t ir_state = 0;
+    uint8_t idx = 0;
+    uint16_t sample_time = 0;
+    
+    for(idx = 0; idx < NUM_SENSORS; idx++)
+    {
+        //set initial val to max for comparison later
+        ir_vals[idx] = MAX_SAMPLE_TIME;
+        
+        //build write val for discharging caps based on number of sensors
+        ir_state |= (1 << idx);
+    }
+    
+    //set bus to output
+    ir_bus.output();
+    //discharge caps
+    ir_bus.write(ir_state);
+    
+    wait_us(10);
+    
+    //set bus to input
+    ir_bus.input();
+    //ensure no pull-up-down resistors to interfere with reading
+    ir_bus.mode(PullNone);
+    
+    //clear sample count and timer
+    sample_cnt = 0;
+    ir_timer.reset();
+    ir_timer.start();
+    
+    do
+    {
+        sample_cnt++;
+        //get sample time from start in micro seconds
+        sample_time = ir_timer.read_us();
+        //get bus state
+        ir_state = ir_bus.read();
+        
+        for(idx = 0; idx < NUM_SENSORS; idx++)
+        {
+            //if pin state switched to 'low', record sample time
+            if(!(ir_state & (1 << idx)) && (ir_vals[idx] > sample_time))
+            {
+                ir_vals[idx] = sample_time;
+            }
+        }
+    }
+    while(sample_time < MAX_SAMPLE_TIME);
+    
+    ir_timer.stop();
+    
+    return(ir_state);
+}