Stefan Krickl / Controller

bla

Controller.cpp@0:7ab090cd6520, 2017-07-13 (annotated)

Committer:
skrickl
Date:
Thu Jul 13 13:42:50 2017 +0000
Revision:
0:7ab090cd6520
bla

Who changed what in which revision?

UserRevisionLine numberNew contents of line
skrickl 0:7ab090cd6520 1 #include "mbed.h"
skrickl 0:7ab090cd6520 2 #include "Controller.h"
skrickl 0:7ab090cd6520 3 #include "Pid_control.h"
skrickl 0:7ab090cd6520 4 #include "Steering_sensor.h"
skrickl 0:7ab090cd6520 5 #include "Actuation.h"
skrickl 0:7ab090cd6520 6 #include "Motor.h"
skrickl 0:7ab090cd6520 7 #include "Brake.h"
skrickl 0:7ab090cd6520 8
skrickl 0:7ab090cd6520 9 Controller::Controller()
skrickl 0:7ab090cd6520 10 {
skrickl 0:7ab090cd6520 11 Motor motor;
skrickl 0:7ab090cd6520 12 Brake brake;
skrickl 0:7ab090cd6520 13 Steering steering;
skrickl 0:7ab090cd6520 14 Pid_control pid_control_speed;
skrickl 0:7ab090cd6520 15 Pid_control pid_control_steering;
skrickl 0:7ab090cd6520 16
skrickl 0:7ab090cd6520 17 _drive_direction=0;
skrickl 0:7ab090cd6520 18 _drive_direction_old=0;
skrickl 0:7ab090cd6520 19 _drive_direction_change=0;
skrickl 0:7ab090cd6520 20
skrickl 0:7ab090cd6520 21 _target_speed=0;
skrickl 0:7ab090cd6520 22 _actual_speed=0;
skrickl 0:7ab090cd6520 23
skrickl 0:7ab090cd6520 24 _target_steering_angle=0;
skrickl 0:7ab090cd6520 25 _actual_steering_angle=0;
skrickl 0:7ab090cd6520 26 _u_steering =0;
skrickl 0:7ab090cd6520 27 _u_throttle =0;
skrickl 0:7ab090cd6520 28 }
skrickl 0:7ab090cd6520 29
skrickl 0:7ab090cd6520 30 Controller::~Controller()
skrickl 0:7ab090cd6520 31 {
skrickl 0:7ab090cd6520 32 }
skrickl 0:7ab090cd6520 33
skrickl 0:7ab090cd6520 34
skrickl 0:7ab090cd6520 35
skrickl 0:7ab090cd6520 36
skrickl 0:7ab090cd6520 37 int Controller::update(int Status)
skrickl 0:7ab090cd6520 38 {
skrickl 0:7ab090cd6520 39
skrickl 0:7ab090cd6520 40 //_target_speed =getVal...
skrickl 0:7ab090cd6520 41 //_actual_speed =getVal...
skrickl 0:7ab090cd6520 42 //_target_steering_angle = getVal...
skrickl 0:7ab090cd6520 43 //_actual_steering_angle =getVal...
skrickl 0:7ab090cd6520 44
skrickl 0:7ab090cd6520 45
skrickl 0:7ab090cd6520 46
skrickl 0:7ab090cd6520 47 //detects target drive direction
skrickl 0:7ab090cd6520 48 if( _target_speed < 0 )
skrickl 0:7ab090cd6520 49 {
skrickl 0:7ab090cd6520 50 _drive_direction = -1;
skrickl 0:7ab090cd6520 51 }
skrickl 0:7ab090cd6520 52 else
skrickl 0:7ab090cd6520 53 {
skrickl 0:7ab090cd6520 54 _drive_direction = 1;
skrickl 0:7ab090cd6520 55 }
skrickl 0:7ab090cd6520 56
skrickl 0:7ab090cd6520 57 //checks if drive direction was changed
skrickl 0:7ab090cd6520 58 if( _drive_direction_old != _drive_direction)
skrickl 0:7ab090cd6520 59 {
skrickl 0:7ab090cd6520 60 _drive_direction_change =1;
skrickl 0:7ab090cd6520 61 }
skrickl 0:7ab090cd6520 62 //stops vehicle
skrickl 0:7ab090cd6520 63 if (_drive_direction_change || _target_speed < min_speed|| Status == 0)
skrickl 0:7ab090cd6520 64 {
skrickl 0:7ab090cd6520 65 stop();
skrickl 0:7ab090cd6520 66
skrickl 0:7ab090cd6520 67 if (_actual_speed < min_speed) // stops with engaged brakes
skrickl 0:7ab090cd6520 68 {
skrickl 0:7ab090cd6520 69 _drive_direction_change=0; // reset if vehicle has stopped
skrickl 0:7ab090cd6520 70
skrickl 0:7ab090cd6520 71 }
skrickl 0:7ab090cd6520 72
skrickl 0:7ab090cd6520 73 }
skrickl 0:7ab090cd6520 74 else
skrickl 0:7ab090cd6520 75 {
skrickl 0:7ab090cd6520 76 // Pid Control for Speed
skrickl 0:7ab090cd6520 77 _u_throttle= pid_control_speed.control(abs(_target_speed),abs(_actual_speed), 30.0,0,0,0,100);
skrickl 0:7ab090cd6520 78
skrickl 0:7ab090cd6520 79
skrickl 0:7ab090cd6520 80 }
skrickl 0:7ab090cd6520 81
skrickl 0:7ab090cd6520 82 //PID Control for Steering Angle, no I needed mechanic system has integrating behavior
skrickl 0:7ab090cd6520 83 _u_steering= pid_control_steering.control(_target_steering_angle, _actual_steering_angle, 10.0,0,0,0,100);
skrickl 0:7ab090cd6520 84
skrickl 0:7ab090cd6520 85 actuate(_u_throttle);
skrickl 0:7ab090cd6520 86 steering.turn(_u_steering);
skrickl 0:7ab090cd6520 87
skrickl 0:7ab090cd6520 88 _drive_direction_old = _drive_direction;
skrickl 0:7ab090cd6520 89
skrickl 0:7ab090cd6520 90 return 0;
skrickl 0:7ab090cd6520 91
skrickl 0:7ab090cd6520 92 }
skrickl 0:7ab090cd6520 93
skrickl 0:7ab090cd6520 94 void Controller::actuate(int u)
skrickl 0:7ab090cd6520 95 {
skrickl 0:7ab090cd6520 96
skrickl 0:7ab090cd6520 97
skrickl 0:7ab090cd6520 98 if(u > 0) // accelerate
skrickl 0:7ab090cd6520 99 {
skrickl 0:7ab090cd6520 100 motor.drive(u, _drive_direction);
skrickl 0:7ab090cd6520 101 brake.brake(0, 100.0);
skrickl 0:7ab090cd6520 102 // set Analog output
skrickl 0:7ab090cd6520 103 }
skrickl 0:7ab090cd6520 104 else //deccelaerate
skrickl 0:7ab090cd6520 105 {
skrickl 0:7ab090cd6520 106 if( u > braking_p1 <= braking_p1)
skrickl 0:7ab090cd6520 107 {
skrickl 0:7ab090cd6520 108 brake.brake(0, 100.0);
skrickl 0:7ab090cd6520 109 }
skrickl 0:7ab090cd6520 110 else if( u > braking_p1)
skrickl 0:7ab090cd6520 111 {
skrickl 0:7ab090cd6520 112 brake.brake(u, speed_brake_1);
skrickl 0:7ab090cd6520 113 }
skrickl 0:7ab090cd6520 114 else if( u > braking_p2)
skrickl 0:7ab090cd6520 115 {
skrickl 0:7ab090cd6520 116 brake.brake(u, speed_brake_2);
skrickl 0:7ab090cd6520 117 }
skrickl 0:7ab090cd6520 118 else if( u > braking_p3)
skrickl 0:7ab090cd6520 119 {
skrickl 0:7ab090cd6520 120 brake.brake(u, speed_brake_3);
skrickl 0:7ab090cd6520 121 }
skrickl 0:7ab090cd6520 122 }
skrickl 0:7ab090cd6520 123
skrickl 0:7ab090cd6520 124
skrickl 0:7ab090cd6520 125
skrickl 0:7ab090cd6520 126
skrickl 0:7ab090cd6520 127 }
skrickl 0:7ab090cd6520 128
skrickl 0:7ab090cd6520 129 void Controller::stop(void)
skrickl 0:7ab090cd6520 130 {
skrickl 0:7ab090cd6520 131 brake.brake(100,100);// max braking, Brake global instance
skrickl 0:7ab090cd6520 132 }