Diff: main.cpp

Revision:

1:fec1d091fa34

Parent:

0:30ff725706d2

Child:

2:eb4101b574bc

--- a/main.cpp	Tue Mar 05 01:19:30 2013 +0000
+++ b/main.cpp	Wed Mar 06 13:58:03 2013 +0000
@@ -49,6 +49,7 @@
 float kp=0;
 float kd=0;
 float ki=0;
+float c=0;
 
 //For PID
 const float timeint = 0.1;
@@ -136,7 +137,7 @@
 int countLeftMissingTeeth = 0,countRightMissingTeeth = 0; 
 
 //Ratio between times of consecutive pulses - to check for missing teeth - (will be >=2 in case of missing teeth)
-float ratio;
+float ratio,ratio1;
 
 //Missing Teeth FMEA Call Function and corresponding Ticker
 
@@ -162,27 +163,31 @@
     {
         if (displayLeftrpm)
         {
-            printf("time_elapsed_Left %f \n",time_elapsed_Left);
+            printf("time_elapsed_Left %f, %f\n",rpm_Left,time_elapsed_Left);
             displayLeftrpm = false;
         }
+        /*
         if (displayLeftdiff)
         {
             printf("CurrentDiffLeftMissingTeethTimeElapsed %d \n",CurrentDiffLeftMissingTeethTimeElapsed);
             displayLeftdiff = false;
         }
+        */
         if (displayRightrpm)
         {
-            printf("time_elapsed_Right %f \n",time_elapsed_Right);
+            printf("time_elapsed_Right %f, %f\n",rpm_Right,time_elapsed_Right);
             displayRightrpm = false;
         }
+        /*
         if (displayRightdiff)
         {
             printf("CurrentDiffRightMissingTeethTimeElapsed %d \n",CurrentDiffRightMissingTeethTimeElapsed);
             displayRightdiff = false;
-        }  
+        } 
+        */ 
         if (displayDash)
         {
-            printf("Hole is missing - move to open loop %f\n", ratio);
+            printf("\n \n HOLE MISSING OPENLOOP %f \n \n", ratio1);
             displayDash = false;
         }
     }
@@ -195,7 +200,7 @@
  
     if(cnt_Left >= 12)                                                  //Measure time between 12 holes => 1 revolution
     { 
-        Current_time_Left = rpm_timer.read_ms();                        //Read Current Timer Reading
+        Current_time_Left = rpm_timer.read_us();                        //Read Current Timer Reading
         time_elapsed_Left = Current_time_Left - Prev_time_Left;         //Difference between Current and precious Timer readings - time for 1 revolution
         Prev_time_Left = Current_time_Left;                             // Set Current Reading as previous reading for next iteration
         cnt_Left = 0;                                                   //Reset Count
@@ -223,6 +228,7 @@
             
             if (ratio > 1.5 || ratio < 1/1.5)                            //If teeth are missing ratio will be >=2
             {
+                ratio1 = ratio;
                 Dash = true;
                 displayDash = true;
             }
@@ -247,7 +253,7 @@
  
     if(cnt_Right >= 12)                                                   //Measure time between 12 holes => 1 revolution
     { 
-        Current_time_Right = rpm_timer.read_ms();                         //Read Current Timer Reading
+        Current_time_Right = rpm_timer.read_us();                         //Read Current Timer Reading
         time_elapsed_Right = Current_time_Right - Prev_time_Right;        //Difference between Current and precious Timer readings - time for 1 revolution
         Prev_time_Right = Current_time_Right;                             // Set Current Reading as previous reading for next iteration
         cnt_Right = 0;                                                    //Reset Count
@@ -298,18 +304,18 @@
     steeringread.read();
     throttleread.read();                                                //read throttle, steering
     
-    throttle=throttleread*5;                                            //calibration : throttle in 1 to 5, stering -110 to 110 degrees
-    steering=220*steeringread-110;
+    throttle=throttleread*5;
+    steering=220*steeringread-110;                                                //calibration : throttle in 1 to 5, stering -110 to 110 degrees                                                                                                                                                                                                                                                                                                                                                    steering=220*steeringread-110;
 
     pid();                                                              // call pid function which gives v1, v2 output
     
     //output pwm for Left wheel   
-    throttle_Left_pulsewidth = rpm_Left;
+    throttle_Left_pulsewidth = throttle_Left*1000/5;
     throttle_Left_pwmout.pulsewidth_us(throttle_Left_pulsewidth);
     throttle_Left_pwmout.period_us(1000);
     
     //output pwm for Right wheel
-    throttle_Right_pulsewidth = rpm_Right;
+    throttle_Right_pulsewidth = throttle_Right*1000/5;
     throttle_Right_pwmout.pulsewidth_us(throttle_Right_pulsewidth);
     throttle_Right_pwmout.period_us(1000);    
 }