Project

General

Profile

Statistics
| Revision:

root / trunk / code / projects / libdragonfly / motor.c @ 1462

History | View | Annotate | Download (5.42 KB)

1
/**
2
 * Copyright (c) 2007 Colony Project
3
 * 
4
 * Permission is hereby granted, free of charge, to any person
5
 * obtaining a copy of this software and associated documentation
6
 * files (the "Software"), to deal in the Software without
7
 * restriction, including without limitation the rights to use,
8
 * copy, modify, merge, publish, distribute, sublicense, and/or sell
9
 * copies of the Software, and to permit persons to whom the
10
 * Software is furnished to do so, subject to the following
11
 * conditions:
12
 * 
13
 * The above copyright notice and this permission notice shall be
14
 * included in all copies or substantial portions of the Software.
15
 * 
16
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
17
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES
18
 * OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
19
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT
20
 * HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY,
21
 * WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
22
 * FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
23
 * OTHER DEALINGS IN THE SOFTWARE.
24
 **/
25

    
26

    
27
/**
28
 * @file motor.c
29
 * @brief Motors
30
 *
31
 * Implementation of functions for controlling the motors.
32
 *
33
 * @author Colony Project, CMU Robotics Club
34
 * Much of this is taken from FWR's library, author: Tom Lauwers
35
 **/
36

    
37
#include "dragonfly_defs.h"
38
#include "motor.h"
39

    
40
/**
41
 * @defgroup motors Motors
42
 * @brief Functions for controlling the motors.
43
 * Functions for controlling the motors. Found in motor.h.
44
 * @{
45
 **/
46

    
47
unsigned char motors_initd=0;
48

    
49
/**
50
 * Initializes both motors so that they can be used with future
51
 * calls to motor1_set and motor2_set.
52
 *
53
 * @return 0 if init succesfull, an error code otherwise
54
 *
55
 * @see motors_off, motor1_set, motor2_set
56
 **/
57
int motors_init( void ) {
58

    
59
  if(motors_initd)
60
    return ERROR_INIT_ALREADY_INITD;
61

    
62

    
63
  // Configure counter such that we use phase correct
64
  // PWM with 8-bit resolution
65
  PORTA &= 0x0F;
66
  DDRA |= 0xF0;
67
  DDRB |= 0x60;
68

    
69
  //timer 1A and 1B
70
  TCCR1A = _BV(COM1A1) | _BV(COM1B1) | _BV(WGM10);
71
  TCCR1B = _BV(WGM12) | _BV(CS10);
72
  //        TCCR1A = 0xA1;
73
  //        TCCR1B = 0x04;
74
  OCR1AH=0;
75
  OCR1AL=0;
76
  OCR1BH=0;
77
  OCR1BL=0;
78

    
79
  motors_initd=1;
80
  return 0;
81
}
82

    
83
/**
84
 * Sets the speed and direction of the left motor.
85
 * motors_init must be called before this function can be used.
86
 *
87
 * @param direction Either FORWARD or BACKWARD to set the direction of rotation.
88
 * @param speed The speed the motor will run at, in the range 0-255.
89
 * 
90
 * @return 0 if init succesfull, an error code otherwise
91
 *
92
 * @see motor_r_set, motors_init
93
 **/
94
int motor_l_set(int direction, int speed) {
95
  if(!motors_initd)
96
    return ERROR_LIBRARY_NOT_INITD;
97

    
98
  if(direction == 0) {
99
    // turn off PWM first if switching directions
100
    if((PORTA & 0x30) != 0x10) {
101
      OCR1A = 0;
102
    }        
103
    PORTA = (PORTA & 0xCF) | 0x10;
104
    //                PORTD |= 0x10;
105
    //                PORTD &= 0xBF;
106
  } else {
107
    // turn off PWM first if switching directions
108
    if((PORTA & 0x30) != 0x20) {
109
      OCR1A = 0;
110
    }
111
    PORTA = (PORTA & 0xCF) | 0x20;
112
    //                PORTD |= 0x40;
113
    //                PORTD &= 0xEF;
114
  }
115
        
116
  // Set the timer to count up to speed, an 8-bit value
117
  OCR1AL = speed;
118

    
119
  return 0;
120
}
121

    
122
/**
123
 * Sets the speed and direction of the right motor.
124
 * motors_init must be called before this function can be used.
125
 *
126
 * @param direction Either FORWARD or BACKWARD to set the direction of rotation.
127
 * @param speed The speed the motor will run at, in the range 0-255.
128
 *
129
 * @return 0 if init succesfull, an error code otherwise
130
 *
131
 * @see motor_l_set, motors_init
132
 **/
133
int motor_r_set(int direction, int speed) {
134
  if(!motors_initd)
135
    return ERROR_LIBRARY_NOT_INITD;
136

    
137
  if(direction == 0) {
138
    //                PORTD |= 0x20;
139
    //                PORTD &= 0x7F;
140
    // turn off PWM first if switching directions
141
    if((PORTA & 0xC0) != 0x80) {
142
      OCR1B = 0;
143
    }                
144
                
145
    PORTA = (PORTA & 0x3F) | 0x80;
146
  } else {
147
    //                PORTD |= 0x80;
148
    //                PORTD &= 0xDF;
149

    
150
    // turn off PWM first if switching directions
151
    if((PORTA & 0xC0) != 0x40) {
152
      OCR1B = 0;
153
    }                
154
                
155
    PORTA = (PORTA & 0x3F) | 0x40;
156
  }
157
  OCR1BL = speed;
158

    
159
  return 0;
160
}
161

    
162
/**
163
 * Sets the speed and direction of motor1.
164
 * motors_init must be called before this function can be used.
165
 *
166
 * @param direction Either FORWARD or BACKWARD to set the direction of rotation.
167
 * @param speed The speed the motor will run at, in the range 0-255.
168
 *
169
 * @return 0 if init succesfull, an error code otherwise
170
 *
171
 * @see motor2_set, motors_init
172
 **/
173
int motor1_set(int direction, int speed) {
174
  return motor_l_set(direction, speed);
175
}
176

    
177
/**
178
 * Sets the speed and direction of motor2.
179
 * motors_init must be called before this function can be used.
180
 *
181
 * @param direction Either FORWARD or BACKWARD to set the direction of rotation.
182
 * @param speed The speed the motor will run at, in the range 0-255.
183
 *
184
 * @return 0 if init succesfull, an error code otherwise
185
 *
186
 * @see motor2_set, motors_init
187
 **/
188
int motor2_set(int direction, int speed) {
189
  return motor_r_set(direction, speed);
190
}
191

    
192

    
193
/**
194
 * Turns off both motors.
195
 *
196
 * @return 0 if init succesfull, an error code otherwise
197
 *
198
 * @see motors_init
199
 **/
200
int motors_off( void ) {
201
  if(!motors_initd)
202
    return ERROR_LIBRARY_NOT_INITD;
203

    
204
  OCR1AL = 0x0;
205
  OCR1BL = 0x0;
206

    
207
  return 0;
208
}
209

    
210
/**@}**///end defgroup
211