scoutece / paintboard / code / motor.c @ 11c945d9
History | View | Annotate | Download (1.98 KB)
1 |
#include <avr/io.h> |
---|---|
2 |
#include "motor.h" |
3 |
#include <stdlib.h> |
4 |
|
5 |
/*
|
6 |
|
7 |
motor controller. Timer2 is set to phase-correct PWM mode, with 32 prescaler.
|
8 |
|
9 |
AIN1: PC0
|
10 |
AIN2: PC1
|
11 |
PWMA: PB3 (Timer2) (OCRA)
|
12 |
|
13 |
BIN1: PC2
|
14 |
BIN2: PC3
|
15 |
PWMB: PD3 (Timer2) (OCRB)
|
16 |
|
17 |
*/
|
18 |
|
19 |
int8_t motor1_speed; |
20 |
int8_t motor2_speed; |
21 |
|
22 |
inline uint8_t abs_clamp(int8_t n)
|
23 |
{ |
24 |
if (n == -128) |
25 |
return 127; |
26 |
else if (n < 0) |
27 |
return -n;
|
28 |
else
|
29 |
return n;
|
30 |
} |
31 |
|
32 |
void motor_init(void) |
33 |
{ |
34 |
|
35 |
// WGM2 0b001 (phase-correct PWM, 8-bit)
|
36 |
// COM2A 0b10 (clear OCR2A on compare match, set on BOTTOM)
|
37 |
// COM2B 0b10 (clear OCR2B on compare match, set on BOTTOM)
|
38 |
// CS2 0b011 (32 prescaler)
|
39 |
TCCR2A = _BV(WGM20) | _BV(COM2A1) |_BV(COM2B1); |
40 |
TCCR2B = _BV(CS21) | _BV(CS20); |
41 |
OCR2A = 0;
|
42 |
OCR2B = 0;
|
43 |
|
44 |
// set output pins
|
45 |
DDRC |= _BV(PC0) | _BV(PC1) | _BV(PC2) | _BV(PC3); |
46 |
DDRB |= _BV(PB3); |
47 |
DDRD |= _BV(PD3); |
48 |
|
49 |
set_motor1(0);
|
50 |
set_motor2(0);
|
51 |
} |
52 |
|
53 |
void set_motor1(int8_t speed)
|
54 |
/* Speed must be between -127 and 127 */
|
55 |
{ |
56 |
motor1_speed = speed; |
57 |
OCR2A = abs_clamp(speed)*2;
|
58 |
if(speed>0) //go forwards |
59 |
{ |
60 |
PORTC |= (1<<PC0);
|
61 |
PORTC &= ~(1<<PC1);
|
62 |
PORTC = PORTC & ~(1<<PC1);
|
63 |
} |
64 |
if(speed<0) //go backwards |
65 |
{ |
66 |
PORTC &= ~(1<<PC0);
|
67 |
PORTC |= (1<<PC1);
|
68 |
PORTC = PORTC & ~(1<<PC0);
|
69 |
} |
70 |
if(speed==0) // turn motor off |
71 |
{ |
72 |
PORTC = PORTC & ~(1<<PC1);
|
73 |
PORTC = PORTC & ~(1<<PC0);
|
74 |
} |
75 |
} |
76 |
|
77 |
void set_motor2(int8_t speed)
|
78 |
/* Speed must be between -127 and 127 */
|
79 |
{ |
80 |
motor2_speed = speed; |
81 |
OCR2B = abs_clamp(speed)*2;
|
82 |
if(speed>0) //go forwards |
83 |
{ |
84 |
PORTC |= (1<<PC2);
|
85 |
PORTC &= ~(1<<PC3);
|
86 |
PORTC = PORTC & ~(1<<PC3);
|
87 |
} |
88 |
if(speed<0) //go backwards |
89 |
{ |
90 |
PORTC &= ~(1<<PC2);
|
91 |
PORTC |= (1<<PC3);
|
92 |
PORTC = PORTC & ~(1<<PC2);
|
93 |
} |
94 |
if(speed==0) // turn motor off |
95 |
{ |
96 |
PORTC = PORTC & ~(1<<PC3);
|
97 |
PORTC = PORTC & ~(1<<PC2);
|
98 |
} |
99 |
} |
100 |
|
101 |
int8_t get_motor1() |
102 |
{ |
103 |
return motor1_speed;
|
104 |
} |
105 |
|
106 |
int8_t get_motor2() |
107 |
{ |
108 |
return motor2_speed;
|
109 |
} |