Mit Hilfe des IR Sensors und dem Servo Motor wird ein Sektor eingescannt. Hält man seine Hand in den gescannten Bereich sollte die Anzahl der gezeigten Finger ausgegeben werden.
#include <avr/io.h> #include <util/delay.h> #include <stdlib.h> #include "adc.h" #include "motors.h" #include "lcd.h" #include "segment_display.h" #include "pinops.h" #include "configuration.h" // Math operations #define DIFF(a, b) ((a) < (b) ? (b) - (a) : (a) - (b)) #define INTEGRATE(value, new_value, n) value = (value * (n - 1) + new_value) / n; // // Main entrance // int main(void) { unsigned short distance, compare_distance = 0; unsigned char matches = 0, num_fingers = 0; signed short servo_pos = -100, scan_dir = 1; char text[10]; // Initialize adc_init(); dcmotors_init(); segment_display_init(); servomotors_init(); setup_output_pin(LED_RED); lcd_init(LCD_DISP_ON); lcd_clrscr(); lcd_puts("Finger counter"); // Endless loop while (1) { // Scan servomotors_position(1, servo_pos); servo_pos += scan_dir; // Servo on edge ? if ((servo_pos < -100) || (servo_pos > 100)) { scan_dir = (servo_pos > 0 ? -1 : 1); // Output results segment_display_write(num_fingers); itoa(num_fingers, text, 10); lcd_gotoxy(0, 1); lcd_puts(text); lcd_puts(" "); // Reset fingers count num_fingers = 0; } // Measure distance distance = adc_sample_value(0, 4); // Check for drastic changes // Look for decreasing distance (increasing sensor output voltage) if (distance > compare_distance + 100) { // Light on finger clear_pin(LED_RED); // inverted! // Changes has to last for some time if (matches++ == 5) { // Count fingers num_fingers++; } } else { // Reset matches and turn off the LED set_pin(LED_RED); // inverted! matches = 0; } // Calculate compare distance (integrated distance) INTEGRATE(compare_distance, distance, 20); // Pause _delay_ms(10); } }
// Hardware pin configurations #define BUTTON_1 PORTPIN(C, 2) #define BUTTON_2 PORTPIN(C, 1) #define BUTTON_3 PORTPIN(C, 0) #define LED_RED PORTPIN(C, 5) #define LED_YELLOW PORTPIN(C, 4) #define LED_GREEN PORTPIN(C, 3) #define DCMOTOR_1A PORTPIN(B, 7) #define DCMOTOR_1B PORTPIN(B, 4) #define DCMOTOR_2A PORTPIN(D, 1) #define DCMOTOR_2B PORTPIN(D, 0) #define DCMOTOR_3A PORTPIN(D, 7) #define DCMOTOR_3B PORTPIN(D, 6) #define DCMOTOR_4A PORTPIN(D, 5) #define DCMOTOR_4B PORTPIN(D, 4) #define SERVOMOTOR_1 PORTPIN(B, 5) #define SERVOMOTOR_2 PORTPIN(B, 6) #define SEGMENT_DISPLAY_LATCH PORTPIN(G, 2) #define SEGMENT_DISPLAY_DATA_OUT PORTPIN(C, 6) #define SEGMENT_DISPLAY_CLOCK PORTPIN(C, 7)
#include <avr/io.h> #include "pinops.h" // // ADC initialization // void adc_init(void) { // ADC setup // Prescaler 8 ADCSRA = BIT(ADEN) | BIT(ADPS1) | BIT(ADPS0); // Reference voltage to external (+5V) ADMUX = BIT(REFS0); } // // ADC conversion waiting // void adc_wait_until_done(void) { while (IS_BIT_SET(ADCSRA, ADSC)) { asm volatile ("nop"); } } // // ADC channel value sampling // unsigned short adc_sample_value(unsigned char channel, unsigned char num_samples) { unsigned short result = 0; // Specify channel ADMUX &= 0xF0; ADMUX |= channel & 0x0F; // Take test sample to "warm up" converter // Usually the first sample is discarded SET_BIT(ADCSRA, ADSC); adc_wait_until_done(); // Real sampling, sum up specifed number of samples for (unsigned char i = 0; i < num_samples;i++) { SET_BIT(ADCSRA, ADSC); adc_wait_until_done(); // Sum-up result += ADCW; } // Return averaged result return (result / num_samples); }
// ADC function headers void adc_init(void); unsigned short adc_sample_value(unsigned char channel, unsigned char num_samples);
#include <avr/io.h> #include "pinops.h" #include "configuration.h" // PWM full period // 14.745600 Mhz / 8 / 50 Hz = 36864 #define PWM_PERIOD (F_CPU / 8 / 50) // PWM servo middle position // 36864 / 20 ms * 1.5 ms = 2764 #define PWM_MIDDLE_POS (PWM_PERIOD * 15 / 200) // PWM ratio to get position from -100 to 100 // 36864 / 20 ms / 2 / 100 // We add +1 to get a full range and a little bit more #define PWM_RATIO (PWM_PERIOD / 20 / 2 / 100 + 1) // // Initialize motors // void dcmotors_init(void) { setup_output_pin(DCMOTOR_1A); setup_output_pin(DCMOTOR_1B); setup_output_pin(DCMOTOR_2A); setup_output_pin(DCMOTOR_2B); setup_output_pin(DCMOTOR_3A); setup_output_pin(DCMOTOR_3B); setup_output_pin(DCMOTOR_4A); setup_output_pin(DCMOTOR_4B); } // // Drive command for specified motor // void dcmotors_drive(unsigned char nr, signed char dir) { switch (nr) { case 1: set_pin_to(DCMOTOR_1A, dir < 0); set_pin_to(DCMOTOR_1B, dir > 0); break; case 2: set_pin_to(DCMOTOR_2A, dir < 0); set_pin_to(DCMOTOR_2B, dir > 0); break; case 3: set_pin_to(DCMOTOR_3A, dir < 0); set_pin_to(DCMOTOR_3B, dir > 0); break; case 4: set_pin_to(DCMOTOR_4A, dir < 0); set_pin_to(DCMOTOR_4B, dir > 0); break; } } // // Intialize servo motors // void servomotors_init(void) { setup_output_pin(SERVOMOTOR_1); setup_output_pin(SERVOMOTOR_2); // Set timer control registers // Clear OUTA and OUTB on compare match // Fast PWM mode with ICR = TOP // Prescaler 8 TCCR1A = BIT(COM1A1) | BIT(COM1B1) | BIT(WGM11); TCCR1B = BIT(CS11) | BIT(WGM13) | BIT(WGM12); // TOP period ICR1 = PWM_PERIOD; } // // Position servo motors // pos is from -100 to 100 // void servomotors_position(unsigned char nr, signed short pos) { // switch (nr) { case 1: OCR1A = PWM_MIDDLE_POS + pos * PWM_RATIO; break; case 2: OCR1B = PWM_MIDDLE_POS + pos * PWM_RATIO; break; } }
// Motor function headers void dcmotors_init(void); void dcmotors_drive(unsigned char nr, signed char dir); void servomotors_init(void); void servomotors_position(unsigned char nr, signed short pos);
#include <avr/io.h> #include <util/delay.h> #include "pinops.h" #include "configuration.h" // // 7 segment display initialization // void segment_display_init(void) { // Set latch, data out and clock pins as output setup_output_pin(SEGMENT_DISPLAY_LATCH); setup_output_pin(SEGMENT_DISPLAY_DATA_OUT); setup_output_pin(SEGMENT_DISPLAY_CLOCK); } // // Digit writing to 7 segment display // void segment_display_write(unsigned char digit) { unsigned char map; // Decimal to segment map switch (digit) { case 0 : map = 0b00111111; break; // Every bit corresponds to one segment case 1 : map = 0b00000110; break; // "1" case 2 : map = 0b01011011; break; // "2" case 3 : map = 0b01001111; break; // "3" and so on case 4 : map = 0b01100110; break; case 5 : map = 0b01101101; break; case 6 : map = 0b01111100; break; case 7 : map = 0b00000111; break; case 8 : map = 0b01111111; break; case 9 : map = 0b01100111; break; default: map = 0b01111001; // E like Error } // Latch low clear_pin(SEGMENT_DISPLAY_LATCH); // Send every bit in the byte. MSB (most significant bit) first. for (signed char i = 7; i >= 0; i--) { // If bit is set, sets the data out pin, otherwise not set_pin_to(SEGMENT_DISPLAY_DATA_OUT, IS_BIT_SET(map, i)); // Clock high for certain period set_pin(SEGMENT_DISPLAY_CLOCK) _delay_us(1); // Clock low for certain period clear_pin(SEGMENT_DISPLAY_CLOCK) _delay_us(1); } // Latch high set_pin(SEGMENT_DISPLAY_LATCH); }
// 7 segment display function headers void segment_display_init(void); void segment_display_write(unsigned char digit);