Statistics
| Branch: | Revision:

root / 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 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
}