-
Notifications
You must be signed in to change notification settings - Fork 2
/
CollisionRecovery.cpp
180 lines (153 loc) · 5.69 KB
/
CollisionRecovery.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
/*
This file is part of the PubSubsumption library, an implementation of the Subsumption
Architecture based upon a simple Publisher/Subscriber mechanism.
Copyright (C) 2014 Terry Crook
Written by Terry Crook in collaboration with Clayton Dean, and based upon the
Subsumption Architecture as described by David P. Anderson.
*/
#include <CollisionRecovery.h>
#include <Director.h>
CollisionRecovery::CollisionRecovery( CommandDispatcher* pCD, uint8_t leftPin, uint8_t rightPin ) : Behavior( pCD )
{
_pName = F("Crash Recover");
_pHelpString = F( " L: Simulate bump left\n"
" R: Simulate bump right\n"
" S: <s1> <s2> <s3> <s4> Speeds (throttle) \n"
" T: <t1> <t2> <t3> <t4> Times (ticks)"
);
// we need to subscribe to events from the two Publishers
SubscribeTo( pCD, 'B' );
_eState = eNormal;
_bSimBumpLeft = false;
_bSimBumpRight = false;
// put some reasonable test values in the state arrays
_StateTimes[ eNormal ] = 0;
_StateTimes[ eStopped ] = 2;
_StateTimes[ eReversing ] = 2;
_StateTimes[ eTurning ] = 2;
_StateTimes[ eForward ] = 2;
_StateSpeeds[ eNormal ] = 0;
_StateSpeeds[ eStopped ] = 0;
_StateSpeeds[ eReversing ] = -5;
_StateSpeeds[ eTurning ] = -10;
_StateSpeeds[ eForward ] = 20;
}
void CollisionRecovery::handleSubsumptionEvent( EventNotification* pEvent, SubsumptionParams* pSubsumptionParams )
{
if ( ! pSubsumptionParams->ControlFreak() ) {
bool bInControl = true;
int leftMotorSpeed = 0;
int rightMotorSpeed = 0;
leftMotorSpeed = _StateSpeeds[_eState];
rightMotorSpeed = _StateSpeeds[_eState];
switch ( _eState ) { // ballistic behavior
case eNormal :
// check the bump sensors
// if we've hit something, stop
if ( _bSimBumpLeft || _bSimBumpRight ) {
leftMotorSpeed = 0;
rightMotorSpeed = 0;
_eState = eStopped;
_nStateTimer = _StateTimes[ _eState ] - 1;
PROGRESS_MSG( "Stopped" );
}
else {
// nothing hit, pass for now . . .
bInControl = false;
}
break;
case eStopped :
if ( --_nStateTimer == 0 ) {
_eState = eReversing;
_nStateTimer = _StateTimes[ _eState ];
PROGRESS_MSG( "Reverse" );
}
break;
case eReversing :
if ( --_nStateTimer == 0 ) {
_eState = eTurning;
_nStateTimer = _StateTimes[ _eState ];
PROGRESS_MSG( "Turning" );
}
break;
case eTurning :
if ( _bSimBumpLeft ) {
leftMotorSpeed = _StateSpeeds[ _eState ] / 2;
}
else {
rightMotorSpeed = _StateSpeeds[ _eState ] / 2;
}
if ( --_nStateTimer == 0 ) {
_eState = eForward;
_nStateTimer = _StateTimes[ _eState ];
PROGRESS_MSG( "Forward" );
}
break;
case eForward :
if ( --_nStateTimer == 0 ) {
_eState = eNormal;
_bSimBumpLeft = false;
_bSimBumpRight = false;
PROGRESS_MSG( "Done" );
}
break;
}
if ( bInControl ) {
pSubsumptionParams->SetThrottles( leftMotorSpeed, rightMotorSpeed, this );
}
}
}
void CollisionRecovery::handleCommandEvent( EventNotification* pEvent, CommandArgs* pArgs )
{
switch( pArgs->inputBuffer[1] ) {
case 0 : // no subcommand
break;
case 'L' : // Simulated Bump left
PROGRESS_MSG("Bump Left!");
_bSimBumpLeft = true;
break;
case 'R' : // Simulated Bump right
PROGRESS_MSG( "Bump Right!");
_bSimBumpRight = true;
break;
case 'S' : // Set recovery state speeds
_StateSpeeds[0] = 0;
for ( int ixArg = 0; ixArg <= 3; ixArg++ ) {
_StateSpeeds[ixArg + 1] = pArgs->nParams[ixArg];
if ( _messageMask & MM_RESPONSES ) {
Serial.print( "Bump speed " );
Serial.print( ixArg + 1 );
Serial.print( " set to " );
Serial.println( _StateSpeeds[ixArg + 1] );
}
}
break;
case 'T' : // Set recovery state times (interval counts)
_StateTimes[0] = 0;
for ( int ixArg = 0; ixArg <= 3; ixArg++ ) {
_StateTimes[ixArg + 1] = pArgs->nParams[ixArg];
if ( _messageMask & MM_RESPONSES ) {
Serial.print( "Bump time " );
Serial.print( ixArg + 1 );
Serial.print( " set to " );
Serial.println( _StateTimes[ixArg + 1] );
}
}
break;
}
}
void CollisionRecovery::PrintSpecificParameterValues()
{
Serial.println( F( "\t\tStop\tBack\tTurn\tFwd" ) );
Serial.print( F( " Speeds:" ) );
for ( int ix = 1; ix <= 4; ix++ ) {
Serial.print( '\t' );
Serial.print( _StateSpeeds[ ix ] );
}
Serial.println();
Serial.print( F( " Times: " ) );
for ( int ix = 1; ix <= 4; ix++ ) {
Serial.print( '\t' );
Serial.print( _StateTimes[ ix ] );
}
}