Revision 25:8bd029d58251, committed 2021-11-06

Comitter:

aalawfi

Date:

Sat Nov 06 16:31:02 2021 +0000

Parent:

24:7bf492bf50f4

Child:

26:54ce9f642477

Commit message:

-

Changed in this revision

--- a/driving.h	Thu Nov 04 17:21:18 2021 +0000
+++ b/driving.h	Sat Nov 06 16:31:02 2021 +0000
@@ -89,7 +89,29 @@
 speedLeftVolt = (speedSensorLeft.read() * 3.3f);
 speedRightVolt = (speedSensorRight.read() * 3.3f);
 
+     
     if(motor_enabled == true) {
+       if(counter == 0 && lap == 0 )
+       {
+           setpointLeft = 0.12;
+           setpointRight = 0.12;
+           }
+        else if(counter == 0 && lap == 2 )
+        {
+            setpointLeft = 0.17;
+            setpointRight = 0.17;
+        }
+        else if(counter !=0 && lap == 2 )
+        {
+            setpointLeft = 0.14;
+           setpointRight = 0.14;
+            
+            }
+        else 
+        {
+        setpointLeft = 0.11;
+        setpointRight = 0.11;    
+        }
        //  setpointLeft = 0.1;
     //     setpointRight = 0.1;
         //--- Calculate Error ---

--- a/main.cpp	Thu Nov 04 17:21:18 2021 +0000
+++ b/main.cpp	Sat Nov 06 16:31:02 2021 +0000
@@ -1,7 +1,7 @@
 #include "MMA8451Q.h"
 #include "mbed.h"
+#include "steering_header.h"
 #include "driving.h"
-#include "steering_header.h"
 #include "steering_methods.h"
 #include "state_control.h"
 
@@ -54,12 +54,11 @@
  //Sets the communication rate of the micro-controller to the Bluetooth module.
 pc.printf("Hello World!\n");
 bt.printf("Hello World!\n");
-
-
+Timer t;
+t.start();
  while(1) {
-     
+     wait(0.2);
      pc.printf("\n\n");
-wait(0.5);
   /*  
      bt.printf("\n\n");
      bt.printf("\n\rMotor enabled ? : %d ", motor_enabled );
@@ -81,11 +80,14 @@
          case COLLISION :      bt.printf("\r\nFault: COLLISION"); break;
          case LOW_VOLTAGE : bt.printf("\r\nFault: LOW VOLTAGE");  break;
           }
-          bt.printf("\n\n----"); 
-          bt.printf("\n\rLeft sensor %1.2f", left_distance_sensor.read());
-          bt.printf("\n\rRight sensor: %1,2", right_distance_sensor.read());
-          bt.printf("\n\rdifference: %1,2", abs(left_distance_sensor.read() - right_distance_sensor.read() ));
-
+          bt.printf("\n\n----");
+       //   bt.printf("%d", t.read_ms());
+          bt.printf("\n\rLeft distance sens (ADC) : %1.5f", left_sens);
+          bt.printf("\n\rRight distance sens (ADC): %1.5f",right_sens);
+          bt.printf("\n\rDifference (ADC): %1.5f",left_sens - right_sens);
+          bt.printf("\n\rFeedback (m) : %1.5f", feedback);
+          bt.printf("\n\rLap: %d", lap);
+         // bt.printf("\n\ravgCellVolt;ag : %1.5f", avgCellVoltag);
 //pot1Voltage = pot1 * 3.3f;
 //dutyCycleLeft = (pot1 * fullBatScalar);
 //dutyCycleRight = (pot1 * fullBatScalar);

--- a/state_control.h	Thu Nov 04 17:21:18 2021 +0000
+++ b/state_control.h	Sat Nov 06 16:31:02 2021 +0000
@@ -49,13 +49,9 @@
     disable_brakes();
     motor_enabled = true; 
     turn_led(GREEN); 
-    setpointLeft = 0.1;
-    setpointRight = 0.1;
     };
 void _wait(void){
      // release brakes, turn on steering system, do not start the motor
-    _fault =false; 
-    fault_type=CLEAR;
      // disable_brakes();
      disable_brakes(); 
     motor_enabled = false;
@@ -85,7 +81,7 @@
     batteryVoltage = battInput * 3.3 * battDividerScalar;
     avgCellVoltage = batteryVoltage / 3.0;
     
-    if (left_distance_sensor.read() < (0.600/3.3) && right_distance_sensor.read()*3.3 < (0.600/3.3))
+    if (left_distance_sensor.read()*1.2 < (0.380/3.3) && right_distance_sensor.read()*1.2 < (0.380/3.3))
         {
           _fault = true;
           fault_type = OFF_TRACK;

--- a/steering_header.h	Thu Nov 04 17:21:18 2021 +0000
+++ b/steering_header.h	Sat Nov 06 16:31:02 2021 +0000
@@ -8,17 +8,17 @@
 #define MIN_TURN_ANGLE -M_PI/4
 #define SERVO_MOTOR_FREQ 50.0f
 #define CENTER_DUTY_CYCLE 0.075f
-#define REF 0.02 //in meters
-#define KP 1.803  // 1.803
+#define REF 0.000 //in meters
 
-#define KD 0.5003 
-#define TIME_DERIVATIVE 0.1 // was 0.08
+float KP = 5.5;//1.2  // 1.5
+#define KI_STEERING 1.2//4.1985
+float KD=  0.2;//0.05003 
 
-#define KI_STEERING 8.397
-#define TIME_INTEGRAL 0.5 // 0.4 
+#define TIME_DERIVATIVE 1 //0.1 // was 0.08
+#define TIME_INTEGRAL 1  //0.5 // 0.4 
 
-#define TI_STEERING 0.02 // in seconds
-#define SEN_DIFF_TO_DIST 0.97 //in [V/m]
+#define TI_STEERING 0.01 // in seconds
+#define SEN_DIFF_TO_DIST 3.5216
 #define BITS 4
 //Construct the Ticker to call the steering control system at a set rate
 // INPUT PINS 
@@ -39,5 +39,5 @@
 // Tickers
 volatile bool steering_enabled = true;
 Ticker steering_control_ticker;
-
-
+int counter=0;
+int lap=0; 

--- a/steering_methods.h	Thu Nov 04 17:21:18 2021 +0000
+++ b/steering_methods.h	Sat Nov 06 16:31:02 2021 +0000
@@ -35,7 +35,7 @@
     }
 //  ------------ LANDMARK DETECTION ------------
 Timer land;
-int counter = 0;
+
 volatile bool landmark_triggered=false; // Debugging purposes only
 volatile bool restarted = false;        // Debugging purposes only
 
@@ -46,23 +46,20 @@
     if(counter >= 15)
         counter =0;
     turn_seg_off(seg1);
-  //  if(right_landmark_sensor.read() == 1 || left_landmark_sensor.read() == 1  )
-  //  { 
-        if(land.read_us() > 1)
-           {
+    if((right_landmark_sensor.read() == 1 || left_landmark_sensor.read() == 1) && land.read_us() > 10  )
+    { 
               landmark_triggered =true; // Debugging purpose only
-            counter++ ;
+              (counter > 6 ? counter = 7 : counter++);
             land.stop();
             }
 
    //  }  
-    if(right_landmark_sensor.read() == 1 && left_landmark_sensor.read() == 1 )
+    if(right_landmark_sensor.read() == 1 && left_landmark_sensor.read() == 1&& land.read_us() > 4)
       {  
-      if(land.read_us() > 1)
-      {  
+        lap++;
         counter = 0;
         restarted=true;  
-        }     // Debugging purposes only
+            // Debugging purposes only
       }
     land.stop();
    show_decimal(counter, seg1);
@@ -83,42 +80,78 @@
 float prev_duty = 0;
 float errorArea;
 float errorAreaPrev =0.0;
+float feedback;
+float err;
+float left_sens; float right_sens;
+volatile bool clamp;
 void steering_control(void)
 {
     
-    float err;
+    
     if(steering_enabled == true ){
         //Read each sensor and calculate the sensor difference
         float sensor_difference = left_distance_sensor.read() -
                                   right_distance_sensor.read();
-                                  
+                                left_sens =   left_distance_sensor.read();
+                                right_sens = right_distance_sensor.read()*1.065;
         //calculate the feedback term for the controller. distance (meters) from
         //track to sensor
-        float feedback = (sensor_difference/SEN_DIFF_TO_DIST); //
-        
+        feedback = (sensor_difference/SEN_DIFF_TO_DIST); //
+       if(abs(feedback) < 0.035 && counter ==0)
+               {
+                    KP = 1.3;
+                    KD = 0.0856;
+                }
+        else if (abs(feedback) > 0.035 && counter ==0)
+            {
+                 KP = 2;
+               
+                }
+        else if (counter == 0 && lap ==2 )
+        {
+            KP=3.3; 
+            KD=0.35;
+        }
+        else 
+           {
+               KP = 5.5;
+               KD = 0.6;
+               }
+          
         //calculate the error term for the controller. distance (meters) from
         //desired position to sensor
          err = REF - feedback; //
 
        //Area of the error: TimeStep*err (width*length of a rectangle)
+       if(clamp == false)
         errorArea= TI_STEERING*err + errorAreaPrev; 
-            
-            
+          
         //Calculate the derivative of the error term for the controller.
         //  e'(t) = (error(t) - error(t-1))/dt
         float errChange = (err - err_prev)/(TI_STEERING);
         
         //Calculate the controller output
         //Angle in radians
-        float servo_angle = (KP*err + KD*errChange*TIME_DERIVATIVE + KI_STEERING*errorArea*TIME_INTEGRAL);
+        float servo_angle = (0.15+KP*err + KD*errChange*TIME_DERIVATIVE + KI_STEERING*errorArea*TIME_INTEGRAL);
         //Calculate the duty cycle for the servo motor
         current_duty_cycle = rad2dc(servo_angle);
         
+          
+             //--- integral anti-windup ---
+        if (servo_angle > 0.1 || servo_angle < 0.05){
+            if (errorArea > 0.0){
+                clamp = true;
+                }
+         } else {
+             clamp = false;
+            }
+     
         servo_motor_pwm.write(current_duty_cycle);
         
       }
       else {
             current_duty_cycle = rad2dc(M_PI/4);
+      
             servo_motor_pwm.write(current_duty_cycle);
           };
     //Save the error for the next calculation.