Diff: Controller.cpp

Revision:

0:7ab090cd6520

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller.cpp	Thu Jul 13 13:42:50 2017 +0000
@@ -0,0 +1,132 @@
+#include "mbed.h"
+#include "Controller.h"
+#include "Pid_control.h"
+#include "Steering_sensor.h"
+#include "Actuation.h"
+#include "Motor.h"
+#include "Brake.h"
+
+Controller::Controller()
+{
+    Motor motor;
+    Brake brake;
+    Steering steering;
+    Pid_control pid_control_speed;
+    Pid_control pid_control_steering;
+  
+    _drive_direction=0;
+    _drive_direction_old=0;
+    _drive_direction_change=0;
+   
+    _target_speed=0;
+    _actual_speed=0;
+   
+    _target_steering_angle=0;
+    _actual_steering_angle=0;
+    _u_steering =0;
+    _u_throttle =0;
+}
+
+Controller::~Controller()
+{
+}
+
+
+
+
+ int Controller::update(int Status)
+    {
+    
+//_target_speed =getVal...
+//_actual_speed =getVal...
+//_target_steering_angle = getVal...
+//_actual_steering_angle =getVal...
+
+
+
+    //detects target drive direction
+    if( _target_speed < 0 )
+    {
+    _drive_direction = -1;
+    }
+    else
+    {
+     _drive_direction = 1;  
+    }
+    
+    //checks if drive direction was changed
+    if( _drive_direction_old != _drive_direction)
+    {
+      _drive_direction_change =1;  
+    }   
+    //stops vehicle
+    if (_drive_direction_change || _target_speed < min_speed|| Status == 0)
+    {
+        stop();
+        
+        if (_actual_speed < min_speed) // stops with engaged brakes
+        {
+           _drive_direction_change=0;  // reset if vehicle has stopped
+
+        }
+    
+    }
+    else
+    {
+        // Pid Control for Speed
+    _u_throttle= pid_control_speed.control(abs(_target_speed),abs(_actual_speed), 30.0,0,0,0,100);
+        
+        
+    }
+    
+    //PID Control for Steering Angle, no I needed mechanic system has integrating behavior
+     _u_steering= pid_control_steering.control(_target_steering_angle, _actual_steering_angle, 10.0,0,0,0,100);
+   
+    actuate(_u_throttle);
+    steering.turn(_u_steering);
+     
+     _drive_direction_old = _drive_direction;
+    
+    return 0;
+    
+    }
+    
+void Controller::actuate(int u)
+    {
+    
+
+    if(u > 0) // accelerate
+    {
+         motor.drive(u, _drive_direction);
+         brake.brake(0, 100.0); 
+        // set Analog output
+    }
+    else //deccelaerate
+    {
+         if( u > braking_p1 <=  braking_p1)
+         {
+             brake.brake(0, 100.0); 
+         }
+         else if( u > braking_p1)
+         {
+             brake.brake(u, speed_brake_1); 
+         }
+         else if(  u > braking_p2)
+         {
+              brake.brake(u, speed_brake_2);
+         }     
+         else if(  u > braking_p3)
+         {
+            brake.brake(u, speed_brake_3);
+         }
+    }
+    
+  
+    
+    
+    }
+    
+      void Controller::stop(void)
+    {
+        brake.brake(100,100);// max braking, Brake global instance
+    }