root / paintboard / code / motor.c @ fcddafaf
History | View | Annotate | Download (1.98 KB)
| 1 | 1226c007 | AnsonLinux | #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 | f4a85021 | Tom Mullins | 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 | 1226c007 | AnsonLinux | 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 | f4a85021 | Tom Mullins | OCR2A = abs_clamp(speed)*2;
|
| 58 | 1226c007 | AnsonLinux | if(speed>0) //go forwards |
| 59 | {
|
||
| 60 | PORTC |= (1<<PC0);
|
||
| 61 | f4a85021 | Tom Mullins | PORTC &= ~(1<<PC1);
|
| 62 | 1226c007 | AnsonLinux | PORTC = PORTC & ~(1<<PC1);
|
| 63 | } |
||
| 64 | if(speed<0) //go backwards |
||
| 65 | {
|
||
| 66 | f4a85021 | Tom Mullins | PORTC &= ~(1<<PC0);
|
| 67 | 1226c007 | AnsonLinux | 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 | f4a85021 | Tom Mullins | OCR2B = abs_clamp(speed)*2;
|
| 82 | 1226c007 | AnsonLinux | if(speed>0) //go forwards |
| 83 | {
|
||
| 84 | PORTC |= (1<<PC2);
|
||
| 85 | f4a85021 | Tom Mullins | PORTC &= ~(1<<PC3);
|
| 86 | 1226c007 | AnsonLinux | PORTC = PORTC & ~(1<<PC3);
|
| 87 | } |
||
| 88 | if(speed<0) //go backwards |
||
| 89 | {
|
||
| 90 | f4a85021 | Tom Mullins | PORTC &= ~(1<<PC2);
|
| 91 | 1226c007 | AnsonLinux | 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 | } |