1 | /*#################################################################################################
|
2 | Name : i2c controlled Motordriver for Lego NXT Motors
|
3 | Version : 0.1 alpha
|
4 | autor : Stefan Huber
|
5 | page :
|
6 | License : GNU General Public License
|
7 |
|
8 |
|
9 | //################################################################################################*/
|
10 | /*Structur of i2c Communication
|
11 | * txbuffer (readable by master)
|
12 | * txbuffer[0] Motor A speed in ticks/sec high byte
|
13 | * txbuffer[1] Motor A speed in ticks/sec low byte
|
14 | * txbuffer[2] Motor A position count high byte
|
15 | * txbuffer[3] Motor A position count low byte
|
16 | * txbuffer[4] Motor B speed in ticks/sec high byte
|
17 | * txbuffer[5] Motor B speed in ticks/sec low byte
|
18 | * txbuffer[6] Motor B position count high byte
|
19 | * txbuffer[7] Motor B position count low byte
|
20 | */
|
21 |
|
22 | /* Pinning of Attiny 24
|
23 | * VCC -> VCC
|
24 | * PB0 -> Clkin (Quarzoszillator) or n.c.
|
25 | * PB1 -> PWM_MOTOR_A0
|
26 | * PB2 -> PWM_MOTOR_A1
|
27 | * PB3 -> RESET -> PULLUP-Resistor -> VCC
|
28 | * PA7 -> TACHO_MOTOR_A0
|
29 | * PA6 -> i2c SDA / ISP MOSI
|
30 | * PA5 -> TACHO_MOTOR_A1 / ISP MISO
|
31 | * PA4 -> i2c SCL / ISP SCK
|
32 | * PA3 -> TACHO_MOTOR_B1
|
33 | * PA2 -> TACHO_MOTOR_B0
|
34 | * PA1 -> PWM_MOTOR_B0
|
35 | * PA0 -> PWM_MOTOR_B1
|
36 | * GND -> GND
|
37 | */
|
38 | //################### Pinning
|
39 | #define DDR_PWM_MOTOR_A DDRB
|
40 | #define DDR_PWM_MOTOR_B DDRA
|
41 | #define DDR_TACHO DDRA
|
42 |
|
43 | #define PORT_PWM_MOTOR_A PORTB
|
44 | #define PORT_PWM_MOTOR_B PORTA
|
45 | #define PIN_TACHO PINA
|
46 |
|
47 | #define PWM_MOTOR_A0 1
|
48 | #define PWM_MOTOR_A1 2
|
49 | #define PWM_MOTOR_B0 1
|
50 | #define PWM_MOTOR_B1 0
|
51 | #define TACHO_MOTOR_A0 7
|
52 | #define TACHO_MOTOR_A1 5
|
53 | #define TACHO_MOTOR_B0 2
|
54 | #define TACHO_MOTOR_B1 3
|
55 |
|
56 | //Additional constants
|
57 | #define TIMEBASE 1000 //1/1000 s or 1 ms is the timebase for all calculations
|
58 | #include <stdlib.h>
|
59 | #include <avr/io.h>
|
60 | #include <avr/interrupt.h>
|
61 | #include <avr/pgmspace.h>
|
62 | #include <util/delay.h>
|
63 | #include <util/atomic.h>
|
64 |
|
65 | //###################### USI-TWI-I2C
|
66 | #include "usiTwiSlave.h"
|
67 | #define SLAVE_ADR_ATTINY_1 0b00110100 // i2c slave address
|
68 | #ifndef F_CPU
|
69 | #define F_CPU 8000000UL
|
70 | #endif
|
71 | //###################### Global Macros
|
72 |
|
73 | #define LOW_BYTE(x) (x & 0xff) // 16Bit --> 8Bit
|
74 | #define HIGH_BYTE(x) ((x >> 8) & 0xff) // 16Bit --> 8Bit
|
75 | //###################### PWM configuration
|
76 | #define F_PWM 50 // PWM-Frequenz in Hz
|
77 | #define PWM_STEPS 256 // PWM-Schritte pro Zyklus(1..256)
|
78 | // ab hier nichts ändern, wird alles berechnet
|
79 |
|
80 | #define T_PWM (F_CPU/(F_PWM*PWM_STEPS)) // Systemtakte pro PWM-Takt
|
81 | #if (T_PWM<(93+5))
|
82 | #error T_PWM zu klein, F_CPU muss vergrösst werden oder F_PWM oder PWM_STEPS verkleinert werden
|
83 | #endif
|
84 | //###################### Global variables
|
85 | volatile uint8_t pwm_settings[4]; //stores speeds for pwm
|
86 | volatile uint16_t tacho_A = 0; // stores tacho count for motor a
|
87 | volatile uint16_t tacho_B = 0; // stores tacho count for motor a
|
88 | volatile uint32_t akt_timebase = 0;
|
89 | volatile uint16_t akt_tachoA = 0;
|
90 | volatile uint16_t akt_tachoB = 0;
|
91 | volatile uint32_t timebase = 0; //stores current time since reset
|
92 |
|
93 |
|
94 | //Interrupt vector that handles tacho interrups
|
95 | ISR(PCINT1_vect)
|
96 | {
|
97 | uint8_t current;
|
98 | uint8_t masked;
|
99 | static uint8_t prev = 0;
|
100 | static uint8_t dir_A = 0;
|
101 | static uint8_t dir_B = 0;
|
102 |
|
103 | current = PIN_TACHO;
|
104 | masked = current ^ prev; //Calculate xor to find out which pins have changed
|
105 |
|
106 | if (masked & (1 << TACHO_MOTOR_A0)) //PIN TACHO_A0 triggers interrupt
|
107 | {
|
108 | if ((prev & (1 << TACHO_MOTOR_A0)) == 0) //only interested in rising edge (prev = 0; current = 1);
|
109 | {
|
110 | if (dir_A)
|
111 | tacho_A--;
|
112 | else
|
113 | tacho_A++;
|
114 | }
|
115 | } else if (masked & (1 << TACHO_MOTOR_A1))
|
116 | dir_A = !dir_A;
|
117 |
|
118 | if (masked & (1 < TACHO_MOTOR_B0)) //PIN TACHO_B0 triggers interrupt
|
119 | {
|
120 | if ((prev & (1 << TACHO_MOTOR_B0)) == 0) //only interested in rising edge (prev = 0; current = 1);
|
121 | {
|
122 | if (dir_B)
|
123 | tacho_B--;
|
124 | else
|
125 | tacho_B++;
|
126 | }
|
127 | } else if (masked & (1 << TACHO_MOTOR_B1))
|
128 | dir_B = !dir_B;
|
129 |
|
130 | prev = current;
|
131 | }
|
132 |
|
133 | //the ISR is called every millisecond
|
134 | ISR(TIM0_COMPA_vect)
|
135 | {
|
136 | timebase++;
|
137 | }
|
138 |
|
139 | ISR(TIM1_COMPA_vect)
|
140 | {
|
141 | static uint8_t pwm_cnt = 0;
|
142 |
|
143 | OCR1A += (uint16_t) T_PWM;
|
144 |
|
145 | if(pwm_settings[0] > pwm_cnt)
|
146 | PORT_PWM_MOTOR_A |= (1<<PWM_MOTOR_A0);
|
147 | else
|
148 | PORT_PWM_MOTOR_A &= ~(1<<PWM_MOTOR_A0);
|
149 |
|
150 | if(pwm_settings[1] > pwm_cnt)
|
151 | PORT_PWM_MOTOR_A |= (1<<PWM_MOTOR_A1);
|
152 | else
|
153 | PORT_PWM_MOTOR_A &= ~(1<<PWM_MOTOR_A1);
|
154 |
|
155 | if(pwm_settings[2] > pwm_cnt)
|
156 | PORT_PWM_MOTOR_B |= (1<<PWM_MOTOR_B0);
|
157 | else
|
158 | PORT_PWM_MOTOR_B &= ~(1<<PWM_MOTOR_B0);
|
159 |
|
160 | if(pwm_settings[3] > pwm_cnt)
|
161 | PORT_PWM_MOTOR_B |= (1<<PWM_MOTOR_B1);
|
162 | else
|
163 | PORT_PWM_MOTOR_B &= ~(1<<PWM_MOTOR_B1);
|
164 |
|
165 | if (pwm_cnt == (uint8_t) (PWM_STEPS - 1))
|
166 | pwm_cnt = 0;
|
167 | else
|
168 | pwm_cnt++;
|
169 | }
|
170 |
|
171 | int main(void) {
|
172 | //Init I2C
|
173 | cli();
|
174 | //Set Motorports as outputs
|
175 | DDR_PWM_MOTOR_A |= (1 << PWM_MOTOR_A0) | (1 << PWM_MOTOR_A1);
|
176 | DDR_PWM_MOTOR_B |= (1 << PWM_MOTOR_B0) | (1 << PWM_MOTOR_B1);
|
177 | //Set Tachoports as inputs
|
178 | DDR_TACHO &= ~((1 << TACHO_MOTOR_A0) | (1 << TACHO_MOTOR_A1) | (1
|
179 | << TACHO_MOTOR_B0) | (1 << TACHO_MOTOR_B1));
|
180 |
|
181 | //Configure Interrupts for Tacho
|
182 | GIMSK = (1 << PCIE0); //Changes on PCINT 7:0 may cause interrupt
|
183 | PCMSK0 = (1 << TACHO_MOTOR_A0) | (1 << TACHO_MOTOR_A1) | (1
|
184 | << TACHO_MOTOR_B0) | (1 << TACHO_MOTOR_B1); //Enable Tachopins for Interrupt source
|
185 |
|
186 |
|
187 | //Configure Timer0 as time base for milliseconds
|
188 | TCCR0A = (1 << WGM01); //CTC Mode
|
189 | TCCR0B = (1 << CS01); //clock prescaler 8
|
190 | OCR0A = ((F_CPU / 8) / TIMEBASE) - 1; //-1 because interrupt is performed in the next cycle
|
191 | TIMSK0 |= (1 << OCIE0A); //Enable Compare interrupt
|
192 |
|
193 | //Configure Timer 1 for Software PWM
|
194 | TCCR1B |= (1 << WGM12); //CTC Mode
|
195 | TCCR1B |= (1 << CS10); //no prescaling
|
196 | TIMSK1 |= (1 << OCIE1A); //Activate Interrupt
|
197 |
|
198 |
|
199 | //Init TWI Slave
|
200 | usiTwiSlaveInit(SLAVE_ADR_ATTINY_1);
|
201 | sei();
|
202 |
|
203 | uint32_t prev_time = 0;
|
204 | uint16_t prev_tachoA = 0;
|
205 | uint16_t prev_tachoB = 0;
|
206 |
|
207 |
|
208 | uint16_t speed_A = 0;
|
209 | uint16_t speed_B = 0;
|
210 |
|
211 | uint16_t delta_time;
|
212 |
|
213 | for (;;) {
|
214 | ATOMIC_BLOCK(ATOMIC_FORCEON)
|
215 | {
|
216 | akt_timebase = timebase;
|
217 | akt_tachoA = tacho_A;
|
218 | akt_tachoB = tacho_B;
|
219 | }
|
220 | delta_time = (uint16_t) (akt_timebase - prev_time);
|
221 | prev_time = akt_timebase;
|
222 |
|
223 | if (delta_time > 0) //to prevent div by zero
|
224 | {
|
225 | speed_A = (akt_tachoA - prev_tachoA) * 1000 / delta_time;
|
226 | speed_B = (akt_tachoB - prev_tachoB) * 1000 / delta_time;
|
227 | }
|
228 | prev_tachoA = akt_tachoA;
|
229 | prev_tachoB = akt_tachoB;
|
230 |
|
231 | //Write everything into the txbuffer
|
232 | txbuffer[0] = HIGH_BYTE(speed_A);
|
233 | txbuffer[1] = LOW_BYTE(speed_A);
|
234 | txbuffer[2] = HIGH_BYTE(tacho_A);
|
235 | txbuffer[3] = LOW_BYTE(tacho_A);
|
236 | txbuffer[4] = HIGH_BYTE(speed_B);
|
237 | txbuffer[5] = LOW_BYTE(speed_B);
|
238 | txbuffer[6] = HIGH_BYTE(tacho_B);
|
239 | txbuffer[7] = LOW_BYTE(tacho_B);
|
240 |
|
241 | //Read speed settings rxbuffer -
|
242 | //TODO: change to access PID controller
|
243 | pwm_settings[0] = rxbuffer[0];
|
244 | pwm_settings[1] = rxbuffer[1];
|
245 | pwm_settings[2] = rxbuffer[2];
|
246 | pwm_settings[3] = rxbuffer[3];
|
247 |
|
248 | }
|
249 |
|
250 | }
|