File content as of revision 0:7ab090cd6520:

#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
    }