-
Notifications
You must be signed in to change notification settings - Fork 2
/
LEDDriver.cpp
165 lines (132 loc) · 6.52 KB
/
LEDDriver.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
/*
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 <LEDDriver.h>
// LED motor emulator
LEDDriver::LEDDriver( uint8_t pwmPinLeft, uint8_t pwmPinRight, uint8_t dirPinLeft, uint8_t dirPinRight, CommandDispatcher* pCD, Position* pOD, float ticksPerInch ) :
Behavior( pCD ),
_ledPwmPinLeft(pwmPinLeft),
_ledPwmPinRight(pwmPinRight),
_ledDirPinLeft(dirPinLeft),
_ledDirPinRight(dirPinRight),
_pPosition( pOD ),
_ticksPerInch( ticksPerInch )
{
_pName = F("LED 'Motor'");
// note that pNextSub is being overwritten here, but this should not be a problem as long as
// the next subscriber for each event is the same, which it should be here.
// if necessary, these could be separate variables, and the appropriate pointer would need
// to be returned in HandleEvent().
SubscribeTo( pCD, 'L' ); // All our commands begin with "L"
_leftRatio = 1.0;
_rightRatio = 1.0; // introduce a differential to simulate extra drag on this side
_throttleChangeLimit = 64; // prevent throttle from changing more than this in each step
pinMode( _ledPwmPinLeft, OUTPUT );
pinMode( _ledPwmPinRight, OUTPUT );
pinMode( _ledDirPinLeft, OUTPUT );
pinMode( _ledDirPinRight, OUTPUT );
_pHelpString = F( " D <LeftRatio> <RightRatio>: Set 'Motor' differential ratios\n"
" L <Limit>: Set throttle change limit\n"
" S <LeftSpeed> <RightSpeed>: Set 'Motor' speeds"
);
}
void LEDDriver::Update( void ) {
/* if ( LED_On_duration_ms > 0 ) {
unsigned int currTime = millis();
if ( currTime > LED_Change_Time ) {
LED_State = !LED_State;
digitalWrite( ledPin, LED_State );
LED_Change_Time = currTime + (LED_State == LOW ? LED_Off_duration_ms : LED_On_duration_ms);
}
}
*/
}
void LEDDriver::handleSubsumptionEvent( EventNotification* pEvent, SubsumptionParams* pSubsumptionParams )
{
if ( _bEnabled ) {
SubsumptionParams* pSubsumptionParams = (SubsumptionParams*) pEvent->pData;
// don't allow rapid throttle changes
_throttleLeft = constrain( pSubsumptionParams->GetLeftThrottle(), _throttleLeft - _throttleChangeLimit, _throttleLeft + _throttleChangeLimit );
_throttleRight = constrain( pSubsumptionParams->GetRightThrottle(), _throttleRight - _throttleChangeLimit, _throttleRight + _throttleChangeLimit );
SetLED( _throttleLeft, _ledPwmPinLeft, _ledDirPinLeft );
SetLED( _throttleRight, _ledPwmPinRight, _ledDirPinRight );
// display name of subsuming Behavior, and requested throttle settings
if ( _messageMask & MM_INFO && pSubsumptionParams->ControlFreak() ) {
Serial.print( '[' );
Serial.print( pSubsumptionParams->ControlFreak()->GetName() );
Serial.print( F( "] " ) );
Serial.print( _throttleLeft );
Serial.print( '/' );
Serial.println( _throttleRight );
}
// simulate Position update. Assume full throttle yields 10 IPS, scale to produce encoder ticks per step
// apply differential ratios to simulate motor/gear/wheel differences in throttle response.
float maxTicksPerStep = _ticksPerInch * 10.0 * ( (float) pSubsumptionParams->GetInterval() / 1000.0 );
_pPosition->_currentEncoderPositionLeft += map( _throttleLeft, 0, 255, 0, maxTicksPerStep ) * _leftRatio;
_pPosition->_currentEncoderPositionRight += map( _throttleRight, 0, 255, 0, maxTicksPerStep ) * _rightRatio;
if ( _messageMask & MM_PROGRESS ) {
PRINT_VAR( maxTicksPerStep );
Serial.print( F( "Positions set to: " ) );
Serial.print( _pPosition->_currentEncoderPositionLeft );
Serial.print( '/' );
Serial.println( _pPosition->_currentEncoderPositionRight );
}
IF_CSV( MM_CSVBASIC ) {
CSV_OUT( _throttleLeft );
CSV_OUT( _throttleRight );
CSV_OUT( pSubsumptionParams->ControlFreak()->GetName() );
}
}
}
void LEDDriver::handleCommandEvent( EventNotification* pEvent, CommandArgs* pArgs )
{
CommandArgs* pData = (CommandArgs*) pEvent->pData;
// check the sub-command.
switch( pData->inputBuffer[1] ) {
case 'S' : // set "speeds"
if ( _bEnabled ) {
int leftSpeed = pData->nParams[0];
int rightSpeed = pData->nParams[1];
SetLED( leftSpeed, _ledPwmPinLeft, _ledDirPinLeft );
SetLED( rightSpeed, _ledPwmPinRight, _ledDirPinRight );
IF_MASK( MM_RESPONSES ) {
Serial.print( F( "LED simulator speeds directly set to: " ) );
Serial.print( leftSpeed ); Serial.print( '\t' );
Serial.println( rightSpeed );
}
}
break;
case 'D' : // set throttle/speed differential
_leftRatio = pData->fParams[0];
_rightRatio = pData->fParams[1];
IF_MASK( MM_RESPONSES ) {
Serial.print( F( "LED simulator throttle/speed ratios set to: " ) );
Serial.print( _leftRatio ); Serial.print( '\t' );
Serial.println( _rightRatio );
}
break;
case 'L' : // set throttle limit
_throttleChangeLimit = pData->nParams[0];
IF_MASK( MM_RESPONSES ) {
Serial.print( F( "LED throttle change limit set to " ) );
Serial.println( _throttleChangeLimit );
}
break;
}
}
// The LED's are bipolar red/green, connected so that driving with one polarity produces green
// and reversing the polarity produces red. One side of the LED pair is connected to a digital
// pin which is set high for forward and low for reverse. The other side is connected to a
// PWM output for dimming to reflect speed. When the direction pin is high, PWM is inverted
// to maintain the correct brightness/speed relationship.
void LEDDriver::SetLED( int speed, int pwmPin, int dirPin )
{
bool bFwd = speed >= 0;
int throttle = constrain( speed, -255, 255 );
analogWrite( pwmPin, bFwd ? throttle : 255 + throttle ); // slightly non-obvious: !bFwd means throttle is negative
digitalWrite( dirPin, !bFwd );
}