/* * Company: Botronix * Author: AJC * Target Device: ATMEGA164P running on the Ranger Robot * Description: This is the source code for the example behavior that is loaded * on Ranger from the factory. It has two behaviors: 1) a system test routine, * and 2) a wander behavior. Selection of the two threads is determined by the * state of the DIP switch out of reset. If DIP 1 and 2 are off, the test thread * is executed, if DIP 1 is on and 2 is off the wander behavior is executed. * * Wander: The robot will drive in a forward direction until a collision is * detected with the front bumper. After a collision has been detected, the * robot will stop, reverse direction, spin to a new direction, and drive * forward. */ #include #include #include //Macros #define bit_set(x,y) (x |= (1 << y)) #define bit_clr(x,y) (x &= ~(1 << y)) //Constants #define TEST_SERVOS 1 #define TEST_LIGHT_SENSORS 0 #define BUMPER_2_LEDS 1 const uint16_t LEFT_STOP = 1520; const uint16_t RIGHT_STOP = 1500; const uint16_t LEFT_FORWARD = 1020 ; const uint16_t RIGHT_FORWARD = 2000; const uint16_t LEFT_REVERSE = 1620 ; const uint16_t RIGHT_REVERSE = 1400; const uint16_t ZTR_LEFT_LEFT = 1620; const uint16_t ZTR_LEFT_RIGHT = 1600; const uint16_t ZTR_RIGHT_LEFT = 1420; const uint16_t ZTR_RIGHT_RIGHT = 1400; //Variables enum direction {STOP, FORWARD, REVERSE, ZTR_RIGHT, ZTR_LEFT}; static int uart_putchar(char c, FILE *stream); static FILE mystdout = FDEV_SETUP_STREAM(uart_putchar, NULL, _FDEV_SETUP_WRITE); volatile uint8_t timer0_ovf_count, buzzer_tone; uint8_t cnt_dir = 1; volatile uint16_t adc_val, adc_val_0, adc_val_1, adc_val_2, adc_val_3, adc_val_4; volatile uint8_t adc_int_flag = 0, adc_chan, blink_flag, mode = 1; /*############################################### Timer/Counter0 Overflow ISR ###############################################*/ ISR(TIMER0_OVF_vect) { timer0_ovf_count++; } /*############################################### Timer/Counter0 Output Compare Match A ISR ###############################################*/ ISR(TIMER0_COMPA_vect) { /* PB3 = Buzzer */ if(mode == 1) //system test { //Toggle buzzer output pin if(bit_is_set(PINB,PB3)) bit_clr(PORTB,PB3); else bit_set(PORTB,PB3); //setup next output compare register interrupt OCR0A += buzzer_tone; } } /*############################################### Timer/Counter1 Output Compare Match A ISR ###############################################*/ ISR(TIMER1_COMPA_vect) { } /*############################################### Timer/Counter1 Output Compare Match B ISR ###############################################*/ ISR(TIMER1_COMPB_vect) { } /*############################################### ADC Conversion Complete ISR ###############################################*/ ISR(ADC_vect) { adc_val = ADCW; ADCSRA &= ~_BV(ADIE); // disable ADC interrupt adc_int_flag = 1; } /*=============================================== InitPorts Initialize I/O Ports ===============================================*/ void InitPorts(void) { /********************************************* PORTA PA0 = Microphone PA1 = ADC1 (Light Sensor Front Left) PA2 = ADC2 (Light Sensor Rear Left) PAA = ADC3 (Light Sensor Front Right) PA4 = ADC4 (Light Sensor Rear Right) PA5 = Dip switch bit0 PA6 = Dip switch bit1 PA7 = Program **********************************************/ PORTA = 0b00011110; // enable input pin pullups (1) DDRA = 0b00000000; //0 is input, 1 is output /********************************************* PORTB PB0 = Left side odometry direction PB1 = Right side odometry direction PB2 = Right side front bumper signal PB3 = Buzzer PB4 = Left side front bumper signal PB5 = SPI MOSI PB6 = SPI MISO PB7 = SPI SCK **********************************************/ //Enable all input pin pullups (1) and set all outputs to zero PORTB = 0b01010111; DDRB = 0b10101000; //0 is input, 1 is output /********************************************* PORTC PC0 = GPIO/I2C clock PC1 = GPIO/I2C data PC2 = LED4/Line detector signal 0 PC3 = LED3/Line detector signal 1 PC4 = LED2/Line detector signal 2 PC5 = LED1/Line detector signal 3 PC6 = Line detector signal 4 PC7 = Line detector signal 5 **********************************************/ //Enable all input pin pullups (1) and set all outputs to zero PORTC = 0b11000011; DDRC = 0b00111100; //0 is input, 1 is output /********************************************* PORTD PD0 = Serial Comms RX PD1 = Serial Comms TX PD2 = Left side odometry trigger PD3 = Right side odometry trigger PD4 = Right side propulsion servo control signal PD5 = Left side propulsion servo control signal PD6 = Servo 0 control signal PD7 = Servo 1 control signal **********************************************/ //Enable input pin pullups (1) and set all outputs to zero PORTD = 0b00001101; DDRD = 0b11110010; //0 is input, 1 is output } /*=============================================== InitUART() Initialize UART ===============================================*/ void InitUART(void) { //UCSR0A: leave default values UCSR0B = _BV(RXEN0) | _BV(TXEN0); //UCSR0C: leave default values //38.4K baud UBRR0H = 0b00000000; UBRR0L = 0b00011001; //115.2K baud //UBRR0H = 0b00000000; //UBRR0L = 0b00001000; } /*=============================================== InitADC Initialize Analog to Digital Converter ===============================================*/ void InitADC(void) { /* Reference Selection = AVcc REFS1 = 0 REFS0 = 1 ADC Left Adjust Result = Disabled ADLAR = 0 Analog Channel and Gain Selection = Single-ended ADC1 (Light Sensor Front Left), 1x Gain MUX4 = 0 MUX3 = 0 MUX2 = 0 MUX1 = 0 MUX0 = 1 ADC Enable = Enable ADEN = 1 ADC Start Conversion = No not start ADSC = 0 ADC Auto Trigger Enable = Disable ADATE = 0 ADC Interrupt Enable = Enable ADIE = 1 ADC Prescaler Select = 128 ADPS2 = 1 ADPS1 = 1 ADPS0 = 1 ADC Auto Trigger Source = Free Running Mode ADTS2 = 0 ADTS1 = 0 ADTS0 = 0 Digital Input Disable ADC7D = 0 ADC6D = 0 ADC5D = 0 ADC4D = 0 ADC3D = 1 ADC2D = 1 ADC1D = 1 ADC0D = 1 */ ADMUX = _BV(REFS0); ADCSRA = _BV(ADEN) | _BV(ADIE) | _BV(ADPS2) | _BV(ADPS1) | _BV(ADPS0); DIDR0 = _BV(ADC3D) | _BV(ADC2D) |_BV(ADC1D) | _BV(ADC0D); adc_chan = 1; } /*=============================================== InitTimer0() Initialize Timer/Counter0 Timer/Counter0 is used to generate blinking LED and buzzer waveforms ===============================================*/ void InitTimer0(void) { /* Compare Match Output A Mode = Normal port operation, OC0A disconnected COM0A1 = 0 COM0A0 = 0 Compare Match Output B Mode = Normal port operation, OC0B disconnected COM0B1 = 0 COM0B0 = 0 Force Output Compare A = off FOC0A = 0 Force Output Compare B = off FOC0B = 0 Wave Form Generation Mode = Normal WGM02 = 0 WGM01 = 0 WGM00 = 0 Clock source = clki/o Prescaler = 1024 CS02 = 1 CS01 = 0 CS00 = 0 Timer/Counter0 Output Compare Match A Interrupt = Disable OCIE0A = 0 Timer/Counter0 Output Compare Match B Interrupt = Disable OCIE0B = 0 Timer/Counter0 Overflow Interrupt = Enable TOIE0 = 1 Drive buzzer with 4KHz square wave from Timer/Counter0 TimerCounter0 overflow interrupt timing: 16Mhz/1024 = 15625Hz 15625Hz/256 = 61Hz 61Hz is well below the needed 4KHz. The output compare match A will be enabled. Set OCR0A = 2 */ TCCR0B = _BV(CS02) | _BV(CS00); TIMSK0 = _BV(OCIE0A) | _BV(TOIE0); // Overflow Interrupt Enable OCR0A = 0x02; } /*=============================================== InitTimer1 Initialize Timer/Counter1 Timer/Counter1 is used to generate waveforms for the propulsion servos ===============================================*/ void InitTimer1(void) { /* Compare Match Output A Mode = Clear OC1A on Compare Match when up-counting Set OC1A on Compare Match when down-counting COM1A1 = 1 COM1A0 = 0 Compare Match Output B Mode = Normal port operation, OC0B disconnected COM1B1 = 1 COM1B0 = 0 Force Output Compare A = off FOC1A = 0 Force Output Compare B = off FOC1B = 0 Wave Form Generation Mode = PWM, Phase and Frequency Correct, TOP = ICR1 WGM13 = 1 WGM12 = 0 WGM11 = 0 WGM10 = 0 Input Noise Canceler = Disable ICNC1 = 0 Input Capture Edge Select = Falling ICNS1 = 0 Clock source = clki/o Prescaler = 8 CS12 = 0 CS11 = 1 CS10 = 0 Timer/Counter1 Output Compare Match A Interrupt = Enable OCIE1A = 1 Timer/Counter1 Output Compare Match B Interrupt = Disable OCIE1B = 1 Timer/Counter0 Overflow Interrupt = Disable TOIE1 = 0 */ //Set ICR1 to 20,000 to start pulse at a rate of 50Hz ICR1 = 20000; //OCR range ~= 1000 to 2000 //Default OCRn to all stop OCR1A = LEFT_STOP; OCR1B = RIGHT_STOP; TCCR1A = _BV(COM1A1) | _BV(COM1B1); TCCR1B = _BV(WGM13) | _BV(CS11); TIMSK1 = _BV(OCIE1B) | _BV(OCIE1A); // Overflow Interrupt Enable } /*=============================================== TestPCComms() Send serial data to PC ===============================================*/ void TestPCComms(void) { uint8_t char_array[10] = {10, 10, 10, 10, 13, 'H','e','l','l','o'}; uint8_t index; for(index = 0; index < 10; index = index + 1) { // Wait for empty transmit buffer while ( !( UCSR0A & (1<= 2000) //start down-count cnt_dir = 0; // if(timer0_ovf_count == 8) if(blink_flag) { //printf("\n\r%i",ocr1a_temp); if(cnt_dir) { cli(); OCR1A += 100; OCR1B -= 100; sei(); } else { cli(); OCR1A -= 100; OCR1B += 100; sei(); } blink_flag = 0; } } /*=============================================== TestLightSensors Test ADC by acquiring light sensor data ===============================================*/ void TestLightSensors(void) { /* PA1 = ADC1 (Light Sensor Front Left) PA2 = ADC2 (Light Sensor Front Right) PAA = ADC3 (Light Sensor Rear Left) PA4 = ADC4 (Light Sensor Rear Right) ADC Start Conversion by writing 1 to ADSC in ADCSRA register */ uint8_t i; if(adc_int_flag) { switch(adc_chan) { case 1: adc_val_1 = adc_val; //Setup for ADC2 ADMUX &= ~_BV(MUX0); ADMUX |= _BV(MUX1); adc_chan = 2; break; case 2: adc_val_2 = adc_val; //Setup for ADC3 ADMUX |= _BV(MUX0); adc_chan = 3; break; case 3: adc_val_3 = adc_val; //Setup for ADC4 ADMUX &= ~_BV(MUX1) & ~_BV(MUX0); ADMUX |= _BV(MUX2); adc_chan = 4; break; case 4: adc_val_4 = adc_val; //Setup for ADC1 ADMUX &= ~_BV(MUX2); ADMUX |= _BV(MUX0); adc_chan = 1; //print results printf("%i\t\t%i\t\t%i\t\t%i", adc_val_1, adc_val_2, adc_val_3, adc_val_4 ); for(i = 0; i < 60; i++) printf("\b"); break; default: //Setup for ADC1 ADMUX &= ~_BV(MUX2); ADMUX |= _BV(MUX0); adc_chan = 1; break; } adc_int_flag = 0; //Start one conversion ADCSRA |= _BV(ADIE); ADCSRA |= _BV(ADSC); } } /*=============================================== TestMic Test Microphone by acquiring sensor data ===============================================*/ void TestMic(void) { /* PA0 = ADC0 (Microphone) ADC Start Conversion by writing 1 to ADSC in ADCSRA register */ uint8_t i; ADMUX &= ~_BV(MUX4) & ~_BV(MUX3) & ~_BV(MUX2) & ~_BV(MUX1) & ~_BV(MUX0); if(adc_int_flag) { adc_val_0 = adc_val/16; printf(" %i", adc_val); for(i = 0; i < 5; i++) printf("\b"); //backspace //printf("\n\r"); // for(i = 0; i < adc_val_0; i++) // printf(" "); // printf("|"); /*for(i = 0; i < (adc_val + 1); i++) printf("\b");*/ adc_int_flag = 0; //Start one conversion ADCSRA |= _BV(ADIE); ADCSRA |= _BV(ADSC); } } /*============================================================================== Sleep16_4ms NOP for ms * 16.4ms ==============================================================================*/ void Sleep16_4ms(uint8_t ms) { timer0_ovf_count = 0; while (timer0_ovf_count != ms); } /*============================================================================== ReadBumper Read bumper sensors, reverse logic, shift the result right, and return result ==============================================================================*/ uint8_t ReadBumper(void) { /* PB2 = Right side front bumper signal PB4 = Left side front bumper signal */ uint8_t pinb_temp; pinb_temp = PINB; pinb_temp = ~pinb_temp; //Bumper switch is active low, make postive logic if(pinb_temp & 0b00000100) { //set bit3 pinb_temp |= 0b00001000; } else { //clear bit3 pinb_temp &= 0b00010000; } pinb_temp = pinb_temp >> 3; pinb_temp &= 0b00000011; return pinb_temp; } /*=============================================== LEDOn Turn LED on ===============================================*/ /* PC2 = LED5/Line detector signal 0 PC3 = LED4/Line detector signal 1 PC4 = LED3/Line detector signal 2 PC5 = LED2/Line detector signal 3 */ void LEDOn(uint8_t on) { switch(on) { case(1): //illum LED1 bit_clr(PORTC,PC5); break; case(2): //illum LED2 bit_clr(PORTC,PC4); break; case(3): //illum LED3 bit_clr(PORTC,PC3); break; case(4): //illum LED4 bit_clr(PORTC,PC2); break; } } /*=============================================== LEDOff Turn LED off ===============================================*/ /* PC2 = LED5/Line detector signal 0 PC3 = LED4/Line detector signal 1 PC4 = LED3/Line detector signal 2 PC5 = LED2/Line detector signal 3 */ void LEDOff(uint8_t off) { switch(off) { case(1): //darken LED1 bit_set(PORTC,PC5); break; case(2): //darken LED2 bit_set(PORTC,PC4); break; case(3): //darken LED3 bit_set(PORTC,PC3); break; case(4): //darken LED4 bit_set(PORTC,PC2); break; } } /*============================================================================ * Maneuver *============================================================================*/ void Maneuver(uint8_t val) { switch(val) { case(STOP):/* stop*/ OCR1A = LEFT_STOP; OCR1B = RIGHT_STOP; break; case(FORWARD):/* forward*/ OCR1A = LEFT_FORWARD; OCR1B = RIGHT_FORWARD; break; case(REVERSE):/* reverse*/ OCR1A = LEFT_REVERSE; OCR1B = RIGHT_REVERSE; break; case(ZTR_RIGHT):/* ztr right*/ OCR1A = ZTR_RIGHT_LEFT; OCR1B = ZTR_RIGHT_RIGHT; break; case(ZTR_LEFT):/* ztr left*/ OCR1A = ZTR_LEFT_LEFT; OCR1B = ZTR_LEFT_RIGHT; break; default:/* stop*/ OCR1A = LEFT_STOP; OCR1B = RIGHT_STOP; break; } } /*=============================================== Drive Perform maneuvers based on bumper sensors ===============================================*/ void Drive(uint8_t bumper) { switch(bumper) { case(0): //no collision, move forward Maneuver(FORWARD); break; case(1):/* collision on the right, move left*/ LEDOn(4); Maneuver(STOP); Sleep16_4ms(15); Maneuver(REVERSE); Sleep16_4ms(90); Maneuver(ZTR_LEFT); Sleep16_4ms(33); //Maneuver(FORWARD); LEDOff(4); break; case(2):/* collision on the left, move right*/ LEDOn(1); Maneuver(STOP); Sleep16_4ms(15); Maneuver(REVERSE); Sleep16_4ms(90); Maneuver(ZTR_RIGHT); Sleep16_4ms(33); //Maneuver(FORWARD); LEDOff(1); break; case(3):/* collision on the right and left, move right*/ LEDOn(1); LEDOn(4); Maneuver(STOP); Sleep16_4ms(15); Maneuver(REVERSE); Sleep16_4ms(90); Maneuver(ZTR_RIGHT); Sleep16_4ms(33); //Maneuver(FORWARD); LEDOff(1); LEDOff(4); break; default: Maneuver(STOP); } } /*=============================================== Test ===============================================*/ void Test(void) { InitADC(); InitTimer0(); InitTimer1(); InitUART(); stdout = &mystdout; TestPCComms(); /* enable interrupts */ sei(); printf("\n\rLED3 is blinking"); printf("\n\rSet DIP switch 1 on to turn on Buzzer"); printf("\n\rToggle DIP switch 2 to change Buzzer's tone"); if (BUMPER_2_LEDS) { printf("\n\rTrigger right bumper to illuminate LED1"); printf("\n\rTrigger left bumper to illuminate LED4"); } if (TEST_SERVOS) { printf("\n\rWheels should alternately moving CW and CCW"); } if (TEST_LIGHT_SENSORS) { printf("\n\n\rLight Sensor inputs:"); printf("\n\rFront Left\tRear Left\tFront Right\tRear Right"); } printf("\n\r"); //Clear LEDs (LEDs are active low!!) PORTC = PORTC | 0b00111100; ADMUX |= _BV(MUX0); //Start one ADC conversion ADCSRA |= _BV(ADIE); ADCSRA |= _BV(ADSC); while(1) { //Call either Bumper2LEDs or DIP2LEDs, but not both simultaneously if (BUMPER_2_LEDS) { Bumper2LEDs(); } else { DIP2LEDs(); } BlinkLED3(); TestBuzzer(); if (TEST_SERVOS) { TestServos(); } if (TEST_LIGHT_SENSORS) { TestLightSensors(); } TestMic(); } } /*=============================================== Wander ===============================================*/ void Wander(void) { uint8_t bumper; uint8_t sleep = 3; InitTimer1(); InitTimer0(); /* enable interrupts */ sei(); //Clear LEDs (LEDs are active low!!) PORTC = PORTC | 0b00111100; //Pause 2 seconds before going Sleep16_4ms(122); //Knight Rider /* while(1) { Sleep16_4ms(sleep); LEDOn(1); Sleep16_4ms(sleep); LEDOn(2); Sleep16_4ms(sleep); LEDOff(1); Sleep16_4ms(sleep); LEDOn(3); Sleep16_4ms(sleep); LEDOff(2); Sleep16_4ms(sleep); LEDOn(4); Sleep16_4ms(sleep); LEDOff(3); Sleep16_4ms(sleep); LEDOn(4); Sleep16_4ms(sleep); LEDOn(3); Sleep16_4ms(sleep); LEDOff(4); Sleep16_4ms(sleep); LEDOn(2); Sleep16_4ms(sleep); LEDOff(3); Sleep16_4ms(sleep); LEDOn(1); Sleep16_4ms(sleep); LEDOff(2); }*/ while(1) { bumper = ReadBumper(); Drive(bumper); } switch(bumper) { case(0): //no collision, move forward break; case(1):/* collision on the right, move left*/ LEDOn(1); Sleep16_4ms(sleep); LEDOff(1); break; case(2):/* collision on the left, move right*/ LEDOn(4); Sleep16_4ms(sleep); LEDOff(4); break; case(3):/* collision on the right and left, move right*/ LEDOn(1); LEDOn(4); Sleep16_4ms(sleep); LEDOff(1); LEDOff(4); break; default: Maneuver(STOP); } } /*=============================================== main ===============================================*/ int main(void) { uint8_t pina_temp; InitPorts(); /* PA5 = Dip switch 1 PA6 = Dip switch 2 */ pina_temp = PINA; pina_temp &= 0b01100000; switch(pina_temp) { case(0b00000000): { mode = 1; Test(); break; } case(0b00100000): { mode = 0; Wander(); break; } default: { while(1); } } return 0; }