« back to all changes in this revision
Viewing changes to atmel/2in1/2in1.c
- browse files at revision 42
- compare with another revision
- download diff
- download tarball
- view history from revision 42
- Committer: Steven Martin
- Date: 2011-05-14 15:06:59 UTC
- Revision ID: steven_martin@live.com.au-20110514150659-ca521pm31eh6oin6
silabs plush10 working
added
removed
atmel/2in1/2in1.c
4
4
//Set your controller here
5
5
#include "../pindefs/tz12a.h"
6
6
7
//Include
7
8
#include <avr/io.h>
8
9
#include <avr/pgmspace.h>
9
10
#include <stdint.h>
14
15
#include <avr/eeprom.h>
15
16
#include "uart.h"
16
17
17
18
//Include pin set functions
19
#include "pinSet.h"
18
20
19
21
#ifndef F_CPU
20
21
22
#define F_CPU 16000000UL
22
23
23
#endif
24
24
25
25
#define unitialised 0
29
29
#define spinLeft 4
30
30
#define spinRight 5
31
31
32
#define OFF 1
33
#define MODE_A 2
34
#define MODE_B 3
35
32
36
volatile uint8_t state = 0;
33
37
volatile uint16_t prevTime = 0;
34
38
volatile uint16_t prevTime1 = 0;
44
48
volatile uint8_t rcTick = 0;
45
49
volatile uint8_t newData1 = 0;
46
50
volatile uint8_t newData2 = 0;
51
volatile uint8_t pwmState = 0;
47
52
48
53
//Stored data variables
49
50
54
volatile uint16_t rcLow = RC_LOW;
51
52
55
volatile uint16_t rcHigh = RC_HIGH;
53
54
56
volatile uint16_t rcMid = (RC_LOW + RC_HIGH)/2;
55
56
57
volatile uint8_t maxSlew = MAX_SLEW;
57
58
58
volatile uint8_t tempLimit = TEMP_LIMIT;
59
60
59
volatile uint8_t goExpo = EXPO;
61
60
62
63
64
61
//Define memory states
65
66
62
uint16_t EEMEM storedRcLow = RC_LOW;
67
68
63
uint16_t EEMEM storedRcHigh = RC_HIGH;
69
70
64
uint8_t EEMEM storedSlewRate = MAX_SLEW;
71
72
65
uint8_t EEMEM storedExpo = EXPO;
73
74
66
uint8_t EEMEM storedTempLimit = TEMP_LIMIT;
75
67
76
77
78
#define LOW_PORT_A LOW_A_PORT
79
#define LOW_PORT_B LOW_B_PORT
80
#define LOW_PORT_C LOW_C_PORT
81
82
#define HIGH_PORT_A HIGH_A_PORT
83
#define HIGH_PORT_B HIGH_B_PORT
84
#define HIGH_PORT_C HIGH_C_PORT
85
86
inline void setALow();
87
88
inline void clrALow();
89
90
inline void setAHigh();
91
92
inline void clrAHigh();
93
inline void setBLow();
94
95
inline void clrBLow();
96
97
inline void setBHigh();
98
99
inline void clrBHigh();
100
inline void setCLow();
101
102
inline void clrCLow();
103
104
inline void setCHigh();
105
106
inline void clrCHigh();
107
108
109
68
//Motor States
110
69
void goForwards();
111
70
void braking();
113
72
void goSpinLeft();
114
73
void goSpinRight();
115
74
116
117
75
//Motor State Functions
118
76
char send_buffer[32];
119
77
int main(){
130
88
SET_LOW_C_PORT |= (1 << LOW_C);
131
89
_delay_ms(100);
132
90
133
134
91
clrALow();
135
136
92
clrAHigh();
137
93
clrBLow();
138
139
94
clrBHigh();
140
141
95
clrCLow();
142
143
96
clrCHigh();
144
97
145
98
DDRD &= ~(1 <<PD3);
146
99
PORTD &= ~(1 << PD3);
147
148
uart_init( UART_BAUD_SELECT(9600,F_CPU) );
100
101
if(UART_DEBUG){
102
uart_init( UART_BAUD_SELECT(9600,F_CPU) );
103
}
149
104
150
105
//Setup Timer
151
106
TCCR0 = 0b00000011; //Setup Timer 0 - scaling of ???? (go check the datasheet and fill it in)
160
115
GICR |= (1 << INT0)|(1 << INT1); //Enable INT0 & INT1
161
116
_delay_ms(100);
162
117
sei();
163
uart_puts("Running UART Debug on 2in1\n");
118
if(UART_DEBUG){
119
uart_puts("Running UART Debug on 2in1\n");
120
}
164
121
//End initialisation
165
122
166
123
167
124
//Main loop
168
125
//Here we manage what mode we are in everything else is done in interrupts
169
while(1){
170
itoa(timeCH1, send_buffer, 10); // convert interger into string (decimal format)
171
172
uart_puts(send_buffer); // and transmit string to UART
173
uart_putc(' ');
174
_delay_ms(10);
175
itoa(timeCH2, send_buffer, 10); // convert interger into string (decimal format)
176
177
uart_puts(send_buffer); // and transmit string to UART
178
uart_putc('\n');
179
126
while(1){
127
128
if(UART_DEBUG){
129
itoa(timeCH1, send_buffer, 10); // convert interger into string (decimal format)
130
uart_puts(send_buffer); // and transmit string to UART
131
uart_putc(' ');
132
_delay_ms(10);
133
itoa(timeCH2, send_buffer, 10); // convert interger into string (decimal format)
134
uart_puts(send_buffer); // and transmit string to UART
135
uart_putc('\n');
136
}
137
/*
180
138
uint8_t deadzone = 40;
181
182
139
uint8_t buffer = 20;
183
184
uint8_t maxOut = 20;
185
140
uint8_t maxOut = 20;
186
141
187
142
if(newData1 == 1 && newData2 == 1){
188
143
//valid pulse
189
144
if ((timeCH1 > (rcLow - buffer)) && (timeCH1 < (rcHigh + buffer)) && (timeCH2 > (rcLow - buffer)) && (timeCH2 < (rcHigh + buffer))){
190
failsafe++;
191
145
failsafe++;
192
146
if (failsafe > 10){ //Need 10 good pulses before we start
193
194
147
failsafe = 15; //Set valid pulse counter (15-10 = 5) therfore we can receive 5 bad pulses before we stop
195
196
148
timeout = 50; //Set timeout counter
197
149
}
198
150
}
206
158
uart_puts("Vaild\n");
207
159
if (rcTick > 0){
208
160
209
//Need to handle states here
210
//Work out the magnitude of the RC signal.
211
212
uint16_t squared = pow((timeCH1/2),2) + pow((timeCH2/2),2); //this gives a squared distance up to 15 bits.
213
//SQRT for linear or just scale it to keep exponential rates
214
uint8_t magnitude = squared >> 7; //Scale to 8 bit from 15.
215
216
//If we are in the center
217
if(magnitude < deadzone){
218
braking();
219
}
220
else if ( abs(timeCH1 - RC_MID) > abs(timeCH2 - RC_MID) ){
221
int16_t normForward = timeCH1 - RC_MID;
222
int16_t normSteer = timeCH2 - RC_MID;
223
224
if (normSteer > deadzone){
225
//normForward/
226
}
227
228
if (abs(timeCH1 - RC_MID) > maxOut){
229
OCR2 = 255;
230
}
231
else {
232
OCR2 = (timeCH1 - RC_MID-deadzone)*256/(RC_HIGH-RC_MID-deadzone);
233
}
234
//Back Forward Mode
235
236
if (timeCH1 > RC_MID){
237
goForwards();
238
239
}
240
else {
241
goBackwards();
242
}
243
244
}
245
246
else {
247
248
//Spinning Mode
249
if (abs(timeCH2 - RC_MID) > maxOut){
250
OCR2 = 255;
251
}
252
else {
253
OCR2 = (timeCH2 - RC_MID-deadzone)*256/(RC_HIGH-RC_MID-deadzone);
254
}
255
256
if (timeCH2 > RC_MID){
257
goSpinRight();
258
}
259
else {
260
goSpinLeft();
261
}
262
263
}
264
265
161
}
266
267
162
}
268
269
163
else {
270
271
164
braking();
272
165
uart_puts("Failsafe\n");
273
274
166
}
275
rcTick = 0;
167
rcTick = 0;
168
*/
276
169
}
277
170
}
278
171
289
182
290
183
291
184
//This interrupt manages RC pulses
292
293
185
ISR(INT0_vect){
294
295
186
//Read in the current time
296
297
187
uint16_t time = TCNT0; //Read lower
298
299
188
time = time + time0*256; //Add upper
300
189
301
302
303
190
if (RC_PORT & ( 1 << RC_PIN)){
304
305
191
prevTime = time; //then save the current time
306
307
192
}
308
309
193
else {
310
311
194
timeCH1 = time - prevTime;
312
195
newData1 = 1;
313
314
196
}
315
316
197
}
317
198
318
199
319
200
ISR(INT1_vect){
320
321
201
//Read in the current time
322
323
202
uint16_t time = TCNT0; //Read lower
324
325
203
time = time + time0*256; //Add upper
326
204
327
328
329
205
if (RC_PORT & ( 1 << RC_PIN2)){
330
331
206
prevTime1 = time; //then save the current time
332
333
207
}
334
335
208
else {
336
337
209
timeCH2 = time - prevTime1;
338
210
newData2 = 1;
339
340
211
}
341
342
212
}
343
213
344
214
345
215
ISR(TIMER2_COMP_vect){ //When comparator is triggered clear low ports (off part of PWM cycle)
346
if (OCR2 <255){
216
if (pwmState == MODE_A){
347
217
if (state != brake){
348
218
//Clear High
349
219
clrAHigh();
354
224
}
355
225
356
226
ISR(TIMER2_OVF_vect ){ //When timer overflows enable low port for current state
357
if (OCR2 > 0){
358
if (state == forward){
359
setAHigh();
360
setCHigh();
227
if ( A > 10 && state != brake){
228
if (state == forward){
229
setAHigh();
230
setCHigh();
231
} else if (state == backward){
232
setALow();
233
setBLow();
234
} else if (state == spinLeft){
235
setCHigh();
236
clrBHigh();
237
} else if (state == spinRight){
238
setAHigh();
239
}
240
OCR2 = A;
241
pwmState = MODE_A;
242
}
243
else if ( B > 10 && state != brake){
244
if (state == forward){
245
clrAHigh();
246
setCHigh();
361
247
} else if (state == backward){
362
248
setBHigh();
363
249
} else if (state == spinLeft){
364
250
setCHigh();
365
251
} else if (state == spinRight){
366
252
setAHigh();
367
} else if (state == brake){
368
setALow();
369
setBLow();
370
setCLow();
371
253
}
254
OCR2 = B;
255
pwmState = MODE_B;
256
}
257
else {
258
372
259
}
373
260
374
if (OCR2 > 250 && state != brake){
375
261
if ((A > 250 || B > 250) && state != brake){
376
262
lets_get_high++;
377
378
263
if (lets_get_high > 50){
379
380
264
//Clear High
381
265
clrAHigh();
382
266
clrBHigh();
383
267
clrCHigh();
384
385
268
lets_get_high = 0;
386
387
269
}
388
389
270
}
390
271
}
391
272
392
273
393
//Motor Pin functions
394
inline void setALow(){
395
396
if (LOW_ACTIVE_LOW){
397
398
LOW_A_PORT &= ~(1 << LOW_A);
399
400
}
401
402
else {
403
404
LOW_A_PORT |= (1 << LOW_A);
405
406
}
407
408
}
409
410
411
412
inline void clrALow(){
413
414
if (LOW_ACTIVE_LOW){
415
416
LOW_A_PORT |= (1 << LOW_A);
417
418
}
419
420
else {
421
422
LOW_A_PORT &= ~(1 << LOW_A);
423
424
}
425
426
}
427
428
429
430
inline void setAHigh(){
431
432
if (HIGH_ACTIVE_LOW){
433
434
HIGH_A_PORT &= ~(1 << HIGH_A);
435
436
}
437
438
else {
439
440
HIGH_A_PORT |= (1 << HIGH_A);
441
442
}
443
444
}
445
446
447
448
inline void clrAHigh(){
449
450
if (HIGH_ACTIVE_LOW){
451
452
HIGH_A_PORT |= (1 << HIGH_A);
453
454
}
455
456
else {
457
458
HIGH_A_PORT &= ~(1 << HIGH_A);
459
460
}
461
462
}
463
464
inline void setBLow(){
465
466
if (LOW_ACTIVE_LOW){
467
468
LOW_B_PORT &= ~(1 << LOW_B);
469
470
}
471
472
else {
473
474
LOW_B_PORT |= (1 << LOW_B);
475
476
}
477
478
}
479
480
481
482
inline void clrBLow(){
483
484
if (LOW_ACTIVE_LOW){
485
486
LOW_B_PORT |= (1 << LOW_B);
487
488
}
489
490
else {
491
492
LOW_B_PORT &= ~(1 << LOW_B);
493
494
}
495
496
}
497
498
499
500
inline void setBHigh(){
501
502
if (HIGH_ACTIVE_LOW){
503
504
HIGH_B_PORT &= ~(1 << HIGH_B);
505
506
}
507
508
else {
509
510
HIGH_B_PORT |= (1 << HIGH_B);
511
512
}
513
514
}
515
516
517
518
inline void clrBHigh(){
519
520
if (HIGH_ACTIVE_LOW){
521
522
HIGH_B_PORT |= (1 << HIGH_B);
523
524
}
525
526
else {
527
528
HIGH_B_PORT &= ~(1 << HIGH_B);
529
530
}
531
532
}
533
534
inline void setCLow(){
535
536
if (LOW_ACTIVE_LOW){
537
538
LOW_C_PORT &= ~(1 << LOW_C);
539
540
}
541
542
else {
543
544
LOW_C_PORT |= (1 << LOW_C);
545
546
}
547
548
}
549
550
551
552
inline void clrCLow(){
553
554
if (LOW_ACTIVE_LOW){
555
556
LOW_C_PORT |= (1 << LOW_C);
557
558
}
559
560
else {
561
562
LOW_C_PORT &= ~(1 << LOW_C);
563
564
}
565
566
}
567
568
569
570
inline void setCHigh(){
571
572
if (HIGH_ACTIVE_LOW){
573
574
HIGH_C_PORT &= ~(1 << HIGH_C);
575
576
}
577
578
else {
579
580
HIGH_C_PORT |= (1 << HIGH_C);
581
582
}
583
584
}
585
586
587
588
inline void clrCHigh(){
589
590
if (HIGH_ACTIVE_LOW){
591
592
HIGH_C_PORT |= (1 << HIGH_C);
593
594
}
595
596
else {
597
598
HIGH_C_PORT &= ~(1 << HIGH_C);
599
600
}
601
602
}
603
604
274
605
275
//Motor States
606
276
void goForwards(){
612
282
clrCLow();
613
283
614
284
_delay_us(200); //Fet switch delay
615
616
285
setBLow(); //Enable Common pin
617
286
618
287
_delay_us(200); //Fet switch delay
619
288
setAHigh(); //Enable one motor
620
621
//Both motors are enabled at slightly different times
622
//to hopefully be a little bit nicer to the fets
623
//200us should have no noticible effect on driving straight
289
624
290
_delay_us(200); //Fet switch delay
625
291
setCHigh; //Enable 2nd motor
626
292
Loggerhead is a web-based interface for Breezy
Version: 2.0.1