Revision 0:7ab090cd6520, committed 2017-07-13

Comitter:

skrickl

Date:

Thu Jul 13 13:42:50 2017 +0000

Commit message:

bla

Changed in this revision

--- /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
+    }

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Controller.h	Thu Jul 13 13:42:50 2017 +0000
@@ -0,0 +1,56 @@
+
+#ifndef CONTROLLER_H
+#define CONTROLLER_H
+
+#include "Pid_control.h"
+#include "Motor.h"
+#include "Brake.h"
+#include "Steering.h"
+
+
+#define max_throttle 50 // 50% maximum throttle
+#define max_braking  50
+#define braking_p1   1.0
+#define braking_p2   2.0
+#define braking_p3   4.0
+#define speed_brake_1 5.0
+#define speed_brake_2 10.0
+#define speed_brake_3 50.0
+#define min_speed     0.5 //at lower speeds vehicle will brake and stop
+
+
+class Controller
+{
+    public:
+    int  update(int Status);
+    
+      Controller();
+    ~Controller();
+   
+    
+    private:
+    
+    Pid_control pid_control_speed;
+    Pid_control pid_control_steering;
+    Motor motor;
+    Brake brake;
+    Steering steering;
+      
+    int _drive_direction;
+    int _drive_direction_old;
+    int _drive_direction_change;
+    int _target_speed;
+    int _actual_speed;
+    int _target_steering_angle;
+    int _actual_steering_angle;
+    int _u_steering;
+    int _u_throttle; 
+
+
+    int Pid_controller(void);
+    void actuate(int u); // value -100 to 100 full braking to full throttle
+    void stop(void);
+    
+    };
+    
+    #endif
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Pid_control.cpp	Thu Jul 13 13:42:50 2017 +0000
@@ -0,0 +1,61 @@
+#include "Pid_control.h"
+
+Pid_control::Pid_control()
+{
+  
+    derivate =0;
+    sum=0.0;
+    delta_t= 0.1;
+    actual_value_old=0;
+}
+
+Pid_control::~Pid_control()
+{
+
+  
+}
+
+
+float Pid_control::control( int actual_value, int target_value, float p, float i,float d,  float min_control_out,  float max_control_out  )
+{
+ 
+
+
+    error = target_value - actual_value;
+
+    // I Anteil
+
+    sum = ((float) error) + sum;
+
+    // D Anteil
+
+    derivate = ((float)(actual_value - actual_value_old))/(delta_t);
+
+    // PID Control
+
+    control_out= p * ((float) error) +  i*sum - d*derivate;
+
+
+
+    if(control_out > max_control_out )
+    {
+        if(i > 0)
+        {
+        sum = sum - (control_out - max_control_out)/i;
+        }
+        control_out =  max_control_out;
+
+    }
+    else if (control_out < min_control_out )
+    {
+        control_out = min_control_out;
+
+    }
+
+
+
+    actual_value_old = actual_value;
+
+    return control_out;
+
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Pid_control.h	Thu Jul 13 13:42:50 2017 +0000
@@ -0,0 +1,28 @@
+#ifndef PID_CONTROL_H
+#define PID_CONTROL_H
+
+class Pid_control
+{
+public:
+
+  
+    Pid_control();
+    ~Pid_control();
+    
+        int error;
+    float control_out;
+    float derivate ;
+    float sum;
+    float delta_t;
+    float actual_value_old;
+
+float control( int actual_value, int target_value, float p, float i,float d,  float min_control_out,  float max_control_out  );
+
+private:
+
+
+  
+
+};
+
+#endif
\ No newline at end of file