Subversion Repositories group.electronics

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
121 pfowler 1 
#include "TinyWireS.h"                  // wrapper class for I2C slave routines
2 
 
3 
#define I2C_SLAVE_ADDR  0x26            // i2c slave address (38)
4 
 
5 
#define  CMD_MAN_BASIC    0x01          // Manually set motor speed/dir (Follows: 3 bytes)
6 
#define  CMD_MAN_TIMED    0x02
7 
#define  CMD_MAN_TIMES    0x03
8 
#define  CMD_MAN_DIST     0x04
9 
 
10 
#define  CMD_GET_RPM      0x0a          // Read RPM's
11 
#define  CMD_GET_DIST     0x0b          // Read Distance's
12 
 
13 
#define  CMD_GET_DEBUG    0x0c
14 
 
15 
// M0 = Left
16 
// M1 = Right
17 
 
18 
#define  OPT_M0DIR    0
19 
#define  OPT_M1DIR    1
20 
#define  OPT_M0TIME   2
21 
#define  OPT_M1TIME   3
22 
#define  OPT_M0DIST   4
23 
#define  OPT_M1DIST   5
24 
 
25 
#define  ON     1
26 
#define  OFF    0
27 
 
28 
#define  TANK_WHEEL_CIRCUM  0x96      // 1 turn of wheel = 150mm travelled per track
29 
#define  TANK_WHEEL_TICK    0x1E      // Distance between magnets
30 
 
31 
#define  M0A    0      // Motor0 +
32 
#define  M0B    1      // Motor0 -
33 
#define  M1A    2      // Motor1 +
34 
#define  M1B    3      // Motor1 -
35 
#define  SCL    4
36 
#define  LED_S1 5      // Led Status
37 
#define  SDA    6
38 
#define  PWM1   7      // Motor1 PWM
39 
#define  PWM0   8      // Motor2 PWM
40 
#define  HE0    10      // Hall Effect 0 PCINT8
41 
#define  HE1    9     // Hall Effect 1 PCINT9
42 
 
43 
const uint8_t  MLEFT = 0;
44 
const uint8_t  MRIGHT = 1;
45 
 
46 
const uint8_t  MFORWARD = 0;
47 
const uint8_t  MREVERSE = 1;
48 
const uint8_t  MRELEASE = 2;
49 
 
50 
uint8_t cmd = 0;
51 
uint8_t options = 0;
52 
 
53 
uint8_t m0Spd = 0;
54 
uint8_t m1Spd = 0;
55 
uint32_t m0Time = 0;
56 
uint32_t m1Time = 0;
57 
byte byteRcvd = 0;
58 
 
59 
uint8_t he0Count = 0;
60 
uint8_t he1Count = 0;
61 
uint8_t rpm0 = 0;
62 
uint8_t rpm0T = 0;
63 
uint8_t rpm1 = 0;
64 
uint8_t rpm1T = 0;
65 
 
66 
uint16_t m0distrav = 0;
67 
uint16_t m1distrav = 0;
68 
 
69 
uint16_t m0MovDis = 0;
70 
uint16_t m1MovDis = 0;
71 
 
72 
uint8_t pcInt1Trig = 0;
73 
uint8_t pcInt1Curr = 0;
74 
uint8_t pcInt1Last = 0;
75 
uint8_t pcInt1Mask = 0;
76 
uint8_t pcInt1Chg = 0;
77 
 
78 
void setup(){
79 
  pinMode(LED_S1,OUTPUT);
80 
  digitalWrite(LED_S1,HIGH);
81 
 
82 
  // Setup PWM control of motors
83 
  DDRA |= (1 << PA7);			//Set OC0B to output
84 
  DDRB |= (1 << PB2);			//Set OC0A to output
85 
  TCCR0A |= ((1 << COM0A1) | (1 << COM0B1) | (1 << WGM01) | (1 << WGM00));
86 
 
87 
  pinMode(M0A,OUTPUT);
88 
  pinMode(M0B,OUTPUT);
89 
  pinMode(M1A,OUTPUT);
90 
  pinMode(M1B,OUTPUT);
91 
 
92 
  // Hall Effect sensors
93 
  DDRB |= (1 << PCINT8);     // Set interrupt pins as input
94 
  DDRB |= (1 << PCINT9);
95 
 
96 
  PORTB |= (1 << PCINT8);    // Enable pull-ups
97 
  PORTB |= (1 << PCINT9);
98 
 
99 
  PCMSK1 |= (1 << PCINT8);  //Enable interrupt on PCINT8 (Pin 2)
100 
  PCMSK1 |= (1 << PCINT9);
101 
  sei();                   // Enable interrupts in SREG
102 
  GIMSK |= (1 << PCIE1);    //Enable interrupts period for PCI1 (PCINT8:11)
103 
 
104 
  setM0_Dir(MRELEASE);
105 
  setM1_Dir(MRELEASE);
106 
 
107 
  pcInt1Last = PINB;
108 
 
109 
  RBlink(LED_S1,2);                    // show it's alive
110 
  TinyWireS.begin(I2C_SLAVE_ADDR);      // init I2C Slave mode
111 
 
112 
  update_motors();
113 
}
114 
 
115 
ISR(PCINT1_vect) {
116 
  pcInt1Curr = PINB;
117 
  pcInt1Mask = pcInt1Curr ^ pcInt1Last;
118 
  pcInt1Last = pcInt1Curr;
119 
  pcInt1Trig = 1;
120 
}
121 
 
122 
void loop(){
123 
 
124 
  if (pcInt1Trig == 1) {
125 
    pcInt1Trig = 0;
126 
    doInterrupt();
127 
  }
128 
 
129 
  // Update run times
130 
  if (bitRead(options, OPT_M0TIME)) {
131 
    if (m0Time >= millis()) {
132 
      bitClear(options, OPT_M0TIME);
133 
      m0Spd = 0;
134 
      update_motors();
135 
    }
136 
  }
137 
  if (bitRead(options, OPT_M1TIME)) {
138 
    if (m1Time >= millis()) {
139 
      bitClear(options, OPT_M1TIME);
140 
      m1Spd = 0;
141 
      update_motors();
142 
    }
143 
  }
144 
 
145 
  // Do comms
146 
  if (TinyWireS.available()){           // got I2C input!
147 
    doComms();
148 
  }
149 
  delay(5);
150 
}
151 
 
152 
void doInterrupt() {
153 
  // Screen out non PC pins
154 
  if ((pcInt1Mask &= PCMSK1) == 0)
155 
    return;
156 
 
157 
  // Loop through all pins on port
158 
  // Set those that have changed
159 
  uint8_t biti;
160 
  for (uint8_t i=0; i<8; i++) {
161 
    biti = 0x01 << i;
162 
    // Check that its a PC int pin
163 
    if (biti & pcInt1Mask) {
164 
      pcInt1Chg |= (1 << i);
165 
    }
166 
  }
167 
 
168 
  // HE0 - Left track
169 
  if (bitRead(pcInt1Chg, PCINT8) && !bitRead(pcInt1Curr, PCINT8)) {
170 
 
171 
    //rpm0 = (30*1000/(millis() - rpm0T))/5;
172 
    //rpm0T = millis();
173 
 
174 
    m0distrav += TANK_WHEEL_TICK;
175 
    if (bitRead(options, OPT_M0DIST) && m0distrav >= m0MovDis) {
176 
      //m0distrav = 0;
177 
      m0Spd = 0;
178 
      bitClear(options, OPT_M0DIST);
179 
      update_motors();
180 
    }
181 
  }
182 
 
183 
  // HE1 - Right track
184 
  if (bitRead(pcInt1Chg, PCINT9) && !bitRead(pcInt1Curr, PCINT9)) {
185 
 
186 
    //rpm1 = (30*1000/(millis() - rpm1T))/5;
187 
    //rpm1T = millis();
188 
 
189 
    m1distrav += TANK_WHEEL_TICK;
190 
    if (bitRead(options, OPT_M1DIST) && m1distrav >= m1MovDis) {
191 
      //m1distrav = 0;
192 
      m1Spd = 0;
193 
      bitClear(options, OPT_M1DIST);
194 
      update_motors();
195 
    }
196 
  }
197 
}
198 
 
199 
void doComms() {
200 
    cmd = TinyWireS.receive();     // get the byte from master
201 
    if (cmd == CMD_MAN_BASIC) {
202 
      // Basic options, speed and direction
203 
      options = TinyWireS.receive();
204 
      m0Spd   = TinyWireS.receive();
205 
      m1Spd   = TinyWireS.receive();
206 
      m0distrav = 0;
207 
      m1distrav = 0;
208 
      update_motors();
209 
    } else if (cmd == CMD_MAN_TIMED) {
210 
      // Timed mode, receive 16bit timing
211 
      options = TinyWireS.receive();
212 
      m0Spd   = TinyWireS.receive();
213 
      m1Spd   = TinyWireS.receive();
214 
      m0Time  = TinyWireS.receive() << 8;  // High byte
215 
      m0Time  = TinyWireS.receive();       // Low byte
216 
      m0Time  += millis();			// All the current millis
217 
      m1Time  = TinyWireS.receive() << 8;
218 
      m1Time  = TinyWireS.receive();
219 
      m0Time  += millis();
220 
      m0distrav = 0;
221 
      m1distrav = 0;
222 
      update_motors();
223 
    } else if (cmd == CMD_MAN_TIMES) {
224 
      // Timed mode, receive 8bit timing
225 
      options = TinyWireS.receive();
226 
      m0Spd   = TinyWireS.receive();
227 
      m1Spd   = TinyWireS.receive();
228 
      m0Time  = TinyWireS.receive();
229 
      m1Time  = TinyWireS.receive();
230 
      m0distrav = 0;
231 
      m1distrav = 0;
232 
      update_motors();
233 
    } else if (cmd == CMD_MAN_DIST) {
234 
      // Timed mode, receive 16bit timing
235 
      options = TinyWireS.receive();
236 
      m0Spd   = TinyWireS.receive();
237 
      m1Spd   = TinyWireS.receive();
238 
      m0MovDis  = TinyWireS.receive() << 8;
239 
      m0MovDis  |= TinyWireS.receive();
240 
      m1MovDis  = TinyWireS.receive() << 8;
241 
      m1MovDis  |= TinyWireS.receive();
242 
      m0distrav = 0;
243 
      m1distrav = 0;
244 
      update_motors();
245 
    } else if (cmd == CMD_GET_RPM) {
246 
      TinyWireS.send(rpm0);
247 
      TinyWireS.send(rpm1);
248 
    } else if (cmd == CMD_GET_DIST) {
249 
      TinyWireS.send(m0distrav >> 8);
250 
      TinyWireS.send(m0distrav);
251 
      TinyWireS.send(m1distrav >> 8);
252 
      TinyWireS.send(m1distrav);
253 
    } else if (cmd == CMD_GET_DEBUG) {
254 
      TinyWireS.send(m1MovDis >> 8);
255 
      TinyWireS.send(m1MovDis);
256 
    } else {
257 
      // Kill the whole buffer, dont know what this command is.
258 
      while (TinyWireS.available())
259 
        TinyWireS.receive();
260 
    }
261 
    RBlink(LED_S1,1);
262 
}
263 
 
264 
inline void pwm0(uint8_t tog) {
265 
  TCCR0A |= ((tog << COM0A1));
266 
}
267 
 
268 
inline void pwm1(uint8_t tog) {
269 
  TCCR0A |= ((tog << COM0B1));
270 
}
271 
 
272 
inline void setM1_Spd(uint8_t num){
273 
  OCR0A = num;
274 
}
275 
 
276 
inline void setM0_Spd(uint8_t num){
277 
  OCR0B = num;
278 
}
279 
 
280 
void setM0_Dir (uint8_t dir) {
281 
  if (dir == MFORWARD) {
282 
    digitalWrite(M0A,HIGH);
283 
    digitalWrite(M0B,LOW);
284 
  } else if (dir == MREVERSE) {
285 
    digitalWrite(M0A,LOW);
286 
    digitalWrite(M0B,HIGH);
287 
  } else {
288 
    digitalWrite(M0A,HIGH);
289 
    digitalWrite(M0B,LOW);
290 
    setM0_Spd(0);
291 
  }
292 
}
293 
 
294 
void setM1_Dir (uint8_t dir) {
295 
  if (dir == MFORWARD) {
296 
    digitalWrite(M1A,HIGH);
297 
    digitalWrite(M1B,LOW);
298 
  } else if (dir == MREVERSE) {
299 
    digitalWrite(M1A,LOW);
300 
    digitalWrite(M1B,HIGH);
301 
  } else {
302 
    digitalWrite(M1A,HIGH);
303 
    digitalWrite(M1B,LOW);
304 
    setM1_Spd(0);
305 
  }
306 
}
307 
 
308 
void update_motors() {
309 
       // Start the tracks rolling
310 
       if (m0Spd > 0) {
311 
         pwm0(ON);
312 
         setM0_Spd(m0Spd);
313 
         if (bitRead(options, OPT_M0DIR))
314 
           setM0_Dir(MFORWARD);
315 
         else
316 
           setM0_Dir(MREVERSE);
317 
       } else {
318 
         pwm0(OFF);
319 
         setM0_Dir(MRELEASE);
320 
       }
321 
 
322 
       if (m1Spd > 0) {
323 
         pwm1(ON);
324 
         setM1_Spd(m1Spd);
325 
         if (bitRead(options, OPT_M1DIR))
326 
           setM1_Dir(MFORWARD);
327 
         else
328 
           setM1_Dir(MREVERSE);
329 
       } else {
330 
         pwm1(OFF);
331 
         setM1_Dir(MRELEASE);
332 
       }
333 
}
334 
 
335 
void Blink(byte led, byte times){ // poor man's display
336 
  for (byte i=0; i< times; i++){
337 
    digitalWrite(led,HIGH);
338 
    delay (100);
339 
    digitalWrite(led,LOW);
340 
    delay (75);
341 
  }
342 
}
343 
 
344 
void RBlink(byte led, byte times){ // poor man's display
345 
  for (byte i=0; i< times; i++){
346 
    digitalWrite(led,LOW);
347 
    delay (100);
348 
    digitalWrite(led,HIGH);
349 
    delay (75);
350 
  }
351 
}
352