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
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
|
/*
*
* RepRap, The Replicating Rapid Prototyper Project
*
* http://reprap.org/
*
* RepRap is copyright (C) 2005-6 University of Bath, the RepRap
* researchers (see the project's People webpage), and other contributors.
*
* RepRap is free; you can redistribute it and/or modify it under the
* terms of the GNU Library General Public Licence as published by the
* Free Software Foundation; either version 2 of the Licence, or (at your
* option) any later version.
*
* RepRap is distributed in the hope that it will be useful, but WITHOUT
* ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
* FITNESS FOR A PARTICULAR PURPOSE. See the GNU Library General Public
* Licence for more details.
*
* For this purpose the words "software" and "library" in the GNU Library
* General Public Licence are taken to mean any and all computer programs
* computer files data results documents and other copyright information
* available from the RepRap project.
*
* You should have received a copy of the GNU Library General Public
* Licence along with RepRap (in reports, it will be one of the
* appendices, for example); if not, write to the Free Software
* Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA, or see
*
* http://www.gnu.org/
*
*/
#include "motor.h"
#include "serial.h"
byte PWMPeriod = 255;
volatile static byte currentDirection = 0;
// Non-zero indicates seeking is in progress (and its speed)
volatile static byte seekSpeed = 0;
volatile static byte lastPortB = 0;
// Note: when reversing motor direction, the speed should be set to 0
// and then delayed long enough for the motor to come to rest.
// Failing to do this could result in miscalculated motor position
// because the code bases the calculation on expected direction, not
// actual direction (and the motor may take a little while to actually
// reverse direction).
typedef union {
int ival;
byte bytes[2];
} addressableInt;
volatile static addressableInt currentPosition, seekPosition;
#define CMD_VERSION 0
#define CMD_FORWARD 1
#define CMD_REVERSE 2
#define CMD_SETPOS 3
#define CMD_GETPOS 4
#define CMD_SEEK 5
#define CMD_FREE 6
#define CMD_NOTIFY 7
#define CMD_SYNC 8
#define CMD_CALIBRATE 9
#define CMD_GETRANGE 10
#define CMD_DDA 11
#define CMD_PWMPERIOD 50
#define CMD_PRESCALER 51
void init2()
{
PWMPeriod = 255;
currentDirection = 0;
seekSpeed = 0;
lastPortB = 0;
currentPosition.bytes[0] = 0;
currentPosition.bytes[1] = 0;
seekPosition.bytes[0] = 0;
seekPosition.bytes[1] = 0;
}
#pragma save
#pragma nooverlay
void setSpeed(byte speed, byte direction)
{
if (speed == 0) {
PORTB = 0;
// Also turn off PWM completely
CCP1M2 = 0;
CCP1M3 = 0;
} else {
if (direction == 0)
PORTB = BIN(00010000); // Set forward output enable
else
PORTB = BIN(00100000); // Set reverse output enable
// Turn on PWM if it wasn't already
CCP1M2 = 1;
CCP1M3 = 1;
}
CCPR1L = speed;
if (speed == 255)
PR2 = 0;
else
PR2 = PWMPeriod;
currentDirection = direction;
_asm /// @todo Remove when sdcc bug fixed
BANKSEL _currentPosition
_endasm;
}
#pragma restore
#pragma save
#pragma nooverlay
void motorTick()
{
char changes, current;
// Clear interrupt flag
RBIF = 0;
current = PORTB; // Store so it doesn't change half way through processing
PORTB = current; // properly reset change ??
changes = lastPortB ^ current;
if (changes & 0x80) {
// Our opto-marker changed
if (current & 0x80) {
// If input is set, we hit the opto-marker. If it's not set
// it's come off the marker, and we only want to deal with one
// of them or we'll double increment everything
// Adjust counter appropriately based on last known direction
if (currentDirection)
currentPosition.ival--;
else
currentPosition.ival++;
if (seekSpeed != 0 && currentPosition.ival == seekPosition.ival) {
seekSpeed = 0;
PORTB = 0;
CCPR1L = 0;
}
}
}
lastPortB = current;
_asm /// @todo Remove when sdcc bug fixed
BANKSEL _currentPosition
_endasm;
}
#pragma restore
void processCommand()
{
switch(buffer[0]) {
case CMD_VERSION:
sendReply();
sendDataByte(CMD_VERSION); // Response type 0
sendDataByte(0); // These don't really mean much right now
sendDataByte(2);
endMessage();
break;
case CMD_FORWARD:
// Forward speed
setSpeed(buffer[1], 0);
break;
case CMD_REVERSE:
// Reverse speed
setSpeed(buffer[1], 1);
break;
case CMD_SETPOS:
// Set (reset) position counter
currentPosition.bytes[0] = buffer[1];
currentPosition.bytes[1] = buffer[2];
break;
case CMD_GETPOS:
// Get position counter
sendReply();
sendDataByte(CMD_GETPOS);
sendDataByte(currentPosition.bytes[0]);
sendDataByte(currentPosition.bytes[1]);
endMessage();
break;
case CMD_SEEK:
// Goto position
seekPosition.bytes[0] = buffer[2];
seekPosition.bytes[1] = buffer[3];
if (seekPosition.ival != currentPosition.ival) {
seekSpeed = buffer[1];
if (currentPosition.ival > seekPosition.ival)
setSpeed(seekSpeed, 1);
else
setSpeed(seekSpeed, 0);
}
break;
case CMD_FREE:
// Free motor. There is no torque hold for a DC motor,
// so all we do is switch off
setSpeed(0, 0);
break;
case CMD_NOTIFY:
// To be implemented
break;
case CMD_SYNC:
// To be implemented
break;
case CMD_CALIBRATE:
// To be implemented
break;
case CMD_GETRANGE:
// To be implemented
break;
case CMD_DDA:
// To be implemented
break;
// "Hidden" low level commands
case CMD_PWMPERIOD:
// Set PWM period
PWMPeriod = buffer[1];
PR2 = PWMPeriod;
break;
case CMD_PRESCALER:
// Set timer prescaler
T2CON = BIN(00000100) | (buffer[1] & 3);
break;
}
}
// To work around sdcc issue
void dummy()
{
INTCON = 0;
}
|
