EE445M RTOS
Taken at the University of Texas Spring 2015
Public Member Functions | Private Attributes | Static Private Attributes | List of all members
drive Class Reference
Drive

#include <drivepp.hpp>

Collaboration diagram for drive:
Collaboration graph

Public Member Functions

 drive ()
 
 drive (motor *left, motor *right, percent_t speed=0, Direction direction=FORWARD)
 
void forward (percent_t speed)
 
void backward (percent_t speed)
 
void set (percent_t speed, Direction dir)
 
void stop (void)
 
void start (void)
 
void steer (uint32_t left_sens, uint32_t left_front_sens, uint32_t right_sens, uint32_t right_front_sens, uint32_t front_sens)
 
void reset_history ()
 

Private Attributes

motor * left
 
motor * right
 
int32_t integral_oblique_error
 
int32_t integral_side_error
 
int32_t last_counter
 

Static Private Attributes

static const int32_t kp_oblique_num = 10
 
static const int32_t kp_oblique_denom = 1
 
static const int32_t kp_side_num = 5
 
static const int32_t kp_side_denom = 2
 
static const int32_t ki_oblique_num = 1
 
static const int32_t ki_oblique_denom = 10000
 
static const int32_t ki_side_num = 1
 
static const int32_t ki_side_denom = 2000
 

Detailed Description

Definition at line 19 of file drivepp.hpp.

Constructor & Destructor Documentation

drive::drive ( )

Definition at line 10 of file drivepp.cpp.

10 {}
drive::drive ( motor *  left,
motor *  right,
percent_t  speed = 0,
Direction  direction = FORWARD 
)

Definition at line 12 of file drivepp.cpp.

References integral_oblique_error, left, right, and set().

12  {
13 
14  this->left = left;
15  this->right = right;
16  set(speed, direction);
17  integral_oblique_error = 0;
18 }
drive::set
void set(percent_t speed, Direction dir)
Definition: drivepp.cpp:44
drive::right
motor * right
Definition: drivepp.hpp:22
drive::integral_oblique_error
int32_t integral_oblique_error
Definition: drivepp.hpp:23
drive::left
motor * left
Definition: drivepp.hpp:21

Here is the call graph for this function:

Member Function Documentation

void drive::backward ( percent_t  speed)

Move backward at some percent of full-speed.

Definition at line 38 of file drivepp.cpp.

References BACKWARD, left, right, and motor::set().

38  {
39 
40  left->set(speed, BACKWARD);
41  right->set(speed, BACKWARD);
42 }
drive::right
motor * right
Definition: drivepp.hpp:22
drive::left
motor * left
Definition: drivepp.hpp:21
motor::set
void set(percent_t speed)
Definition: motorpp.cpp:67
BACKWARD
Definition: direction.hpp:8

Here is the call graph for this function:

void drive::forward ( percent_t  speed)

Move forward at some percent of full-speed.

Definition at line 32 of file drivepp.cpp.

References FORWARD, left, right, and motor::set().

32  {
33 
34  left->set(speed, FORWARD);
35  right->set(speed, FORWARD);
36 }
drive::right
motor * right
Definition: drivepp.hpp:22
FORWARD
Definition: direction.hpp:8
drive::left
motor * left
Definition: drivepp.hpp:21
motor::set
void set(percent_t speed)
Definition: motorpp.cpp:67

Here is the call graph for this function:

void drive::reset_history ( )

Definition at line 122 of file drivepp.cpp.

References integral_oblique_error, and integral_side_error.

Referenced by switch_responder().

122  {
123 
124  integral_oblique_error = 0;
125  integral_side_error = 0;
126 }
drive::integral_oblique_error
int32_t integral_oblique_error
Definition: drivepp.hpp:23
drive::integral_side_error
int32_t integral_side_error
Definition: drivepp.hpp:24

Here is the caller graph for this function:

void drive::set ( percent_t  speed,
Direction  dir 
)

Set motor vectors

Definition at line 44 of file drivepp.cpp.

References left, right, and motor::set().

Referenced by drive().

44  {
45 
46  left->set(speed, dir);
47  right->set(speed, dir);
48 }
drive::right
motor * right
Definition: drivepp.hpp:22
drive::left
motor * left
Definition: drivepp.hpp:21
motor::set
void set(percent_t speed)
Definition: motorpp.cpp:67

Here is the call graph for this function:

Here is the caller graph for this function:

void drive::start ( void  )

Re-enable all previoulsy stopped motors

Definition at line 26 of file drivepp.cpp.

References left, right, and motor::start().

Referenced by motor_control().

26  {
27 
28  left->start();
29  right->start();
30 }
drive::right
motor * right
Definition: drivepp.hpp:22
motor::start
void start(void)
Definition: motorpp.cpp:96
drive::left
motor * left
Definition: drivepp.hpp:21

Here is the call graph for this function:

Here is the caller graph for this function:

void drive::steer ( uint32_t  left_sens,
uint32_t  left_front_sens,
uint32_t  right_sens,
uint32_t  right_front_sens,
uint32_t  front_sens 
)

Feed the autonomous driver sensor inputs.

Definition at line 50 of file drivepp.cpp.

References clamp(), FORWARD, integral_oblique_error, integral_side_error, kp_oblique_denom, kp_oblique_num, last_counter, left, motor::pwm_max_period, right, and motor::set().

Referenced by driver().

52  {
53 
54  /* todo: feed the lf/f, rf/r data here for porportional control of
55  * the motors. the side with the larger coefficient slows more */
56 
57  Direction dir = FORWARD;
58 
59 
60  left_sens = 0;
61  right_sens = 0;
62  front_sens = 0;
63 
64  /* if (last_counter >= steps_to_wait) { */
65  /* last_counter = 0; */
66 
67  /* if (abs(last_left_sens - left_sens) > reset_thresh) { */
68  /* integral_oblique_error = 0; */
69  /* integral_side_error = 0; */
70  /* GPIOPinWrite(GPIO_PORTF_BASE, PIN_RED, PIN_RED ^ GPIOPinRead(GPIO_PORTF_BASE, PIN_RED)); */
71  /* } else if(abs(last_left_front_sens - left_front_sens) > reset_thresh) { */
72  /* integral_oblique_error = 0; */
73  /* integral_side_error = 0; */
74  /* GPIOPinWrite(GPIO_PORTF_BASE, PIN_RED, PIN_RED ^ GPIOPinRead(GPIO_PORTF_BASE, PIN_RED)); */
75  /* } else if(abs(last_right_sens - right_sens) > reset_thresh) { */
76  /* integral_oblique_error = 0; */
77  /* integral_side_error = 0; */
78  /* GPIOPinWrite(GPIO_PORTF_BASE, PIN_RED, PIN_RED ^ GPIOPinRead(GPIO_PORTF_BASE, PIN_RED)); */
79  /* } else if(abs(last_right_front_sens - right_front_sens) > reset_thresh) { */
80  /* integral_oblique_error = 0; */
81  /* integral_side_error = 0; */
82  /* GPIOPinWrite(GPIO_PORTF_BASE, PIN_RED, PIN_RED ^ GPIOPinRead(GPIO_PORTF_BASE, PIN_RED)); */
83  /* } */
84 
85  /* last_left_sens = left_sens; */
86  /* last_left_front_sens = left_front_sens; */
87  /* last_right_sens = right_sens; */
88  /* last_right_front_sens = right_front_sens; */
89  /* } */
90 
91  int32_t left_speed = left->pwm_max_period/2;
92  int32_t right_speed = right->pwm_max_period/2;
93 
94  int32_t oblique_error = ((int32_t)left_front_sens - (int32_t)right_front_sens);
95  integral_oblique_error = clamp(integral_oblique_error + oblique_error, -motor::pwm_max_period/4, motor::pwm_max_period/4);
96 
97  int32_t side_error = ((int32_t)left_sens - (int32_t)right_sens);
98  integral_side_error = clamp(integral_side_error + side_error, -motor::pwm_max_period/8, motor::pwm_max_period/8);
99 
100  int32_t should_use_side_sensors = front_sens < 200;
101  should_use_side_sensors = 0;
102 
103  int32_t should_slow_down = front_sens < 200;
104 
105  //int32_t delta = oblique_error*kp_oblique_num/kp_oblique_denom +
106  // (should_use_side_sensors * side_error * kp_side_num/kp_side_denom) +
107  // (integral_oblique_error*ki_oblique_num/ki_oblique_denom) + (integral_side_error*ki_side_num/ki_side_denom);
108  int32_t delta = oblique_error*kp_oblique_num/kp_oblique_denom;
109  //+ (integral_oblique_error*ki_oblique_num/ki_oblique_denom);
110 
111  if (should_slow_down > 0) { delta /= 2; }
112 
113  left_speed += delta;
114  right_speed -= delta;
115 
116  left->set(left_speed, dir);
117  right->set(right_speed, dir);
118 
119  ++last_counter;
120 }
drive::kp_oblique_denom
static const int32_t kp_oblique_denom
Definition: drivepp.hpp:27
drive::right
motor * right
Definition: drivepp.hpp:22
clamp
int32_t clamp(int32_t value, int32_t min, int32_t max)
Definition: math.cpp:5
motor::pwm_max_period
static const uint16_t pwm_max_period
Definition: motorpp.hpp:75
drive::integral_oblique_error
int32_t integral_oblique_error
Definition: drivepp.hpp:23
FORWARD
Definition: direction.hpp:8
Direction
Direction
Definition: direction.hpp:8
drive::integral_side_error
int32_t integral_side_error
Definition: drivepp.hpp:24
drive::left
motor * left
Definition: drivepp.hpp:21
drive::last_counter
int32_t last_counter
Definition: drivepp.hpp:38
motor::set
void set(percent_t speed)
Definition: motorpp.cpp:67
drive::kp_oblique_num
static const int32_t kp_oblique_num
Definition: drivepp.hpp:26

Here is the call graph for this function:

Here is the caller graph for this function:

void drive::stop ( void  )

Stop all motors

Definition at line 20 of file drivepp.cpp.

References left, right, and motor::stop().

Referenced by motor_control(), and Timer0A_Handler().

20  {
21 
22  left->stop();
23  right->stop();
24 }
drive::right
motor * right
Definition: drivepp.hpp:22
motor::stop
void stop(void)
Definition: motorpp.cpp:45
drive::left
motor * left
Definition: drivepp.hpp:21

Here is the call graph for this function:

Here is the caller graph for this function:

Member Data Documentation

int32_t drive::integral_oblique_error
private

Definition at line 23 of file drivepp.hpp.

Referenced by drive(), reset_history(), and steer().

int32_t drive::integral_side_error
private

Definition at line 24 of file drivepp.hpp.

Referenced by reset_history(), and steer().

const int32_t drive::ki_oblique_denom = 10000
staticprivate

Definition at line 33 of file drivepp.hpp.

const int32_t drive::ki_oblique_num = 1
staticprivate

Definition at line 32 of file drivepp.hpp.

const int32_t drive::ki_side_denom = 2000
staticprivate

Definition at line 36 of file drivepp.hpp.

const int32_t drive::ki_side_num = 1
staticprivate

Definition at line 35 of file drivepp.hpp.

const int32_t drive::kp_oblique_denom = 1
staticprivate

Definition at line 27 of file drivepp.hpp.

Referenced by steer().

const int32_t drive::kp_oblique_num = 10
staticprivate

Definition at line 26 of file drivepp.hpp.

Referenced by steer().

const int32_t drive::kp_side_denom = 2
staticprivate

Definition at line 30 of file drivepp.hpp.

const int32_t drive::kp_side_num = 5
staticprivate

Definition at line 29 of file drivepp.hpp.

int32_t drive::last_counter
private

Definition at line 38 of file drivepp.hpp.

Referenced by steer().

motor* drive::left
private

Definition at line 21 of file drivepp.hpp.

Referenced by backward(), drive(), forward(), set(), start(), steer(), and stop().

motor* drive::right
private

Definition at line 22 of file drivepp.hpp.

Referenced by backward(), drive(), forward(), set(), start(), steer(), and stop().

The documentation for this class was generated from the following files: