scoutece / paintboard / code / motor.c @ f4a85021
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 phasecorrect 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 (phasecorrect PWM, 8bit)

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 
} 