/* Ant weight combat robot control program July 16 2003 Copyright 2003 by Dale Alan Heatherington - dale.h@wa4dsy.net Code first used in Amdroid-A 16oz combat robot http://www.wa4dsy.net/robot/ Atmel AVR AT90S8535 microcontroller Compiled with avr-gcc (See Makefile) Based on invertabot.c control pgm . Some functions or variables may be unused since the Ant bot does not have some features that Inverabot has. Aug 2 2003: Improved the integrator function by adding a "do nothing" condition when error is zero and variable slew rate based on size of error. Motor control is much smoother now. This change was made to the Invertabot code too. */ /* This program is free software; you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation; either version 2 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program; if not, write to the Free Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA The GNU General Public License can also be found online at: http://www.fsf.org/licenses/licenses.html#GPL */ /***************************************************************************/ //Note: CPU type defined in Makefile #include #include #include #include #include #include #include #include #define WDTO_250MS 4 /* Watchdog timer value = 250ms */ #define UCHAR uint8_t #define F_CPU 7372800 /* crystal frequency of CPU */ #define TRUE 1 #define FALSE 0 #define SYNC1 0xf0 #define SYNC2 0x55 #define MODE_STOP 0 #define REV 0 #define FWD 1 #define MAX_FWD_SPEED 0xe0 #define MAX_REV_SPEED 0x20 #define HALF_FWD_SPEED 0xf3 #define HALF_REV_SPEED 13 #define QTR_FWD_SPEED 0xf9 #define QTR_REV_SPEED 7 /* Packet: 0xff 0xf0 0x55 ADDRESS COUNT DATA... CRCH CRCL */ #define COUNT 1 /* offset to count field in tx/rx buffers */ #define ADDRESS 0 /* offset to address field in tx/rx buffers */ #define DATA 2 /* offset to start of data in tx/rx buffers */ #define RIGHTJOY_Y 3 //offset to right joystick Y value #define RIGHTJOY_X 4 //offset to right joystick X value #define LEFTJOY_Y 5 #define LEFTJOY_X 6 #define MATRIX0 7 //offset to matrix 0 byte (These are the controller buttons) #define MATRIX1 8 //offset to matrix 1 byte #define BOT_ADDR 1 /* Address of this robot */ //Bits in port C #define TS_UPRIGHT 0 /* input, low true, from mercury switch */ #define TS_INVERTED 1 /* input, low ture, from mercury switch */ #define TARGET_LEFT_LED 2 /* output low true, Yellow left target detected LED */ #define TARGET_CENTER_LED 3 /* output low true, Green center target detected LED */ #define TARGET_RIGHT_LED 4 /* output low true, Yellow center target detected LED */ #define BOOST 6 /* High true. Enables voltage booster for motors */ //Bits in Port D (Outputs) #define LED_RED 2 /* low true, battery voltage low warning */ #define LED_YELLOW 3 /* low true, radio carrier signal present */ #define LED_GREEN 4 /* low true, good data from radio present */ #define LED_RESET 7 /* low true, RESET in progress */ #define LED_IR_RIGHT 5 /* High true, 100us pulse to IR LEDS on right side */ #define LED_IR_LEFT 6 /* High true, 100us pulse to IR LEDS on left side */ /* Switch Matrix bit defs (buttons on controller)*/ /*matrix0*/ #define m_mode 0x80 #define m_east 0x40 #define m_north 0x20 #define m_rumble 0x10 #define m_south 0x08 #define m_s 0x04 #define m_left_fire 0x02 #define m_west 0x01 /*matrix1*/ #define m_right_fire 0x80 #define m_A 0x40 #define m_B 0x20 #define m_C 0x10 #define m_Z 0x08 #define m_Y 0x04 #define m_X 0x02 /*--- Motor control variables and defs ----- */ /* Port B motor control bits for L293D H-Bridge chip*/ #define FwdLeft 2 /*Left Motor forward */ #define RevLeft 3 /*Left Motor reverse*/ #define FwdRight 0 /*Right Motor forward */ #define RevRight 1 /*Right Motor reverse */ #define RM_ENABLE 4 /* Right Motor enable */ #define LM_ENABLE 5 /* LEft Motor enable */ #define P_width 4 /*Motor pulse width in interrupt ticks (52 uS per tick ) */ UCHAR workB; UCHAR r_speed; UCHAR l_speed; UCHAR dir_ctl; UCHAR speed; int right_speed,left_speed; UCHAR right_prm, left_prm; UCHAR dir_flag_left, dir_flag_right; UCHAR bot_mode, testmode; volatile UCHAR timer_led_green; volatile UCHAR timer_mstop; UCHAR inverted; UCHAR rmfb,lmfb; //Motor feedback values from AD conv. , range is 92 - 164 with 128=stopped //lower values equal FORWARD, higher are REVERSE (when bot is upright). UCHAR l_IR,r_IR, amb_light, timer_ir_led; UCHAR boost; #define AD_POT 0 #define AD_IR_PULSE 2 #define AD_RM_SPEED 4 #define AD_LM_SPEED 5 void adjust_motor_speed(); UCHAR speed_to_pulse_rate(int speed); int integrate(UCHAR speed,UCHAR mfb,int integ); /* ------- End of motor control -------------*/ UCHAR matrix0, matrix1, last_matrix0, last_matrix1, test; UCHAR ad_m_current, ad_lm_speed, ad_rm_speed, ad_bat_volt, ad_floor_sense, ad_rssi; //A-D converter values #define RXSIZE 20 UCHAR tx_buf[32]; //Serial tx buffer for debug telemetry UCHAR rx_buf[RXSIZE]; //Serial receive buffer for RC commands UCHAR rx_pkt[RXSIZE-2]; UCHAR tx_ptr,tx_ctr , rx_ptr, rx_ctr; UCHAR txstate,rxstate; uint16_t crc, rxcrc,last_rx; UCHAR counter,val,rssi; UCHAR ir_sens_left, ir_sens_right; UCHAR x_offset, y_offset; volatile UCHAR timer_inhibit_flipper; volatile uint16_t timer_fire_flipper; volatile UCHAR servo_pulse_width, timer_servo, timer_servo_active; UCHAR ad_pot; int ii; UCHAR prescale ; UCHAR prescale100; UCHAR prescale1sec; UCHAR prescaleMinute; UCHAR timer10ms ; UCHAR timer1sec; volatile unsigned int minutes; volatile UCHAR run_seconds; void make_info_pkt(); void set_tx_buffer_crc(); uint16_t get_adc(uint8_t chan); uint16_t calc_crc(char data, uint16_t crc); void make_info_pkt(); void delay_100us(uint8_t count); //------------------------------------------------------------------------- UCHAR hard_limit(UCHAR x, UCHAR lim) { if(x < 0x80){ if(x > lim) x = lim; return x; } if(x >= 0x80){ lim = (lim ^ 0xff) + 1; if(x < lim) x = lim; return x; } } //-------------------------------------------------------------------------- /* Read the 10 bit analog to digital converter */ uint16_t get_adc(UCHAR chan) { outp(chan,ADMUX); //Select channel sbi(ADCSR,ADSC); //Start conversion while( bit_is_clear(ADCSR,ADIF)); //Wait for ADC output registers to be loaded return __inw_atomic(ADCL); //Return 16 bit value } //--------------------------------------------------------------------------- /* Read 10 bit AD converter but return only 8 bits discarding low 2 bits. */ uint8_t get_adc8(UCHAR chan) { uint16_t result; outp(chan,ADMUX); //Select channel sbi(ADCSR,ADSC); //Start conversion while( bit_is_clear(ADCSR,ADIF)); //Wait for ADC output registers to be loaded result = __inw_atomic(ADCL); //Get 10 bit AD value return result >> 2; //Return 8 bit value } //--------------------------------------------------------------------------- // compute 16 bit CRC on 1 data byte. // crc is a variable which accumulates the 16 bit CRC. // It must be cleared before starting to compute 16 bit CRC on a string. // The output value will be returned as a 16 bit int. uint16_t calc_crc(char data, uint16_t crc){ uint8_t i; for (i=0;i<8;i++) { //Do all 8 bits in the data byte if (data & 0x80) crc ^= 0x8000; //if data bit 7 is set then flip crc msb data = data << 1; // get next data bit if (crc & 0x8000) { //if crc msb is 1... crc = crc << 1; // ..shift left 1 crc = crc ^ 0x8005; // ..xor with 0x8005 } else crc = crc << 1; //else just shift left 1 with no xor } return crc; } //----------------------------------------------------------------------- //Compute CRC on the TX packet and put it on the end void set_tx_buffer_crc(){ UCHAR j = tx_buf[COUNT] + 2; //get data size UCHAR i; crc = 0; for (i=0;i> 8); //tack the crc bytes on the end of the buffer tx_buf[i++] = (uint8_t)(crc & 0xff); } //-------------------------------------------------------------------------- //Check CRC on a received packet. If result is zero CRC is good. uint16_t ck_rx_crc(){ uint16_t rx_crc; UCHAR j = rx_buf[COUNT] + 4; //get size of complete packet including crc bytes UCHAR i; rx_crc = 0; for(i=0; i < j; i++) rx_crc = calc_crc(rx_buf[i],rx_crc); //compute CRC over complete buffer return rx_crc; } //--------------------------------------------------------------------------- //Multiply by 1.25 UCHAR mpyx125(UCHAR y) { if(y <= 101){ y = y + (y >> 2); return y; } if(y >= 154){ y = (y ^ 0xff) + 1; y = y + (y >> 2); y = (y ^ 0xff) + 1; } return y; } /*------------------------------------------------------------------------------*/ UCHAR abs_diff(UCHAR a, UCHAR b) { if(a == 0) return b; if(b == 0) return a; if(a > b) return (a - b); else return ( b - a) ; } /*--------------------------------------------------------------------------------*/ //Returns 0 if a = b returns 255 if ratio is infinite //returns ratio otherwise. // eg: a = 2, b = 4 returns 128 = 2:1 // a = 2, b = 6 returns 171 = 3:1 // a = 20 b = 30 returns 85 = 1.5:1 UCHAR ratio(UCHAR a, UCHAR b) { int d; UCHAR c; if(a == b) return 0; if((a == 0) && (b == 0)) return 0; c = abs_diff(a,b); d = c * 256; d = d -1; if(a > b) c = a ; else c = b; if(c == 0) return 255; else return (d / c); } /*---------------------------------------------------------------------------------*/ //IR object detector processing. Normal ambiant light reading is about 50. // Globals r_IR and l_IR get updated here. /* Note to self: It may be wise to split this into two functions, one for left and one for right, then call them in order: lr, rl,lr,rl... This may cancel the effects of intentional synchronous jamming. However, the chances of encountering such jamming is very small. */ #define IRDELAY 40 void process_IR() { UCHAR i; uint16_t result; amb_light = get_adc8(AD_IR_PULSE); //Read ambiant light value if(amb_light < 100){ //Don't process if blinded by external IR pulse cli(); //Interrupts off for(i=0;i> 2; //Return 8 bit value if(l_IR > amb_light) l_IR = l_IR - amb_light; //subtract ambient light else l_IR = 0; } amb_light = get_adc8(AD_IR_PULSE); //Read ambiant light value if(amb_light < 100) { //Don't process if blinded by external IR pulse cli(); //Interrupts off outp(AD_IR_PULSE,ADMUX); //Select channel for(i=0;i> 2; //Return 8 bit value cbi(PORTD,LED_IR_RIGHT); if(r_IR > amb_light) r_IR = r_IR - amb_light; //subtract ambient light else r_IR = 0; } if(inverted){ //If bot is inverted swap left and right IR values i = r_IR; r_IR = l_IR; l_IR = i; } } /*-------------------------------------------------------------------------------------*/ #define lmin 7 #define Ratio_min 120 #define OD_LEFT 1 #define OD_CENTER 2 #define OD_RIGHT 3 char opponent_detect() { char rv; rv = 0; sbi(PORTC,TARGET_LEFT_LED); sbi(PORTC,TARGET_RIGHT_LED); sbi(PORTC,TARGET_CENTER_LED); if((l_IR > lmin) || (r_IR > lmin)){ if(ratio(l_IR,r_IR) > Ratio_min){ if(l_IR > r_IR){ rv = OD_LEFT; cbi(PORTC,TARGET_LEFT_LED) ; } else{ rv = OD_RIGHT; cbi(PORTC,TARGET_RIGHT_LED); } }else{ rv = OD_CENTER; cbi(PORTC,TARGET_CENTER_LED); //Turn on green attack LED } } return rv; } //-------------------------------------------------------------------------- void inversion_check(UCHAR init) { static UCHAR inv_ctr ; /* Inversion detector switch logic. Changes state of "inverted" variable depending on physical position. Port C input Bit 0 is low and bit 1 is high when upright. Switches open when bot is tilted more than 30 degrees. They are positioned 180 degrees from each other . */ if(init == 1){ inv_ctr = 0; //reset debouncer if init is 1 inverted = FALSE; } if((bit_is_set(PINC,0)) && (bit_is_clear(PINC,1))) { //Inversion switch debouncer if(inv_ctr < 50) inv_ctr++; } if((bit_is_clear(PINC,0)) && (bit_is_set(PINC,1))){ if(inv_ctr > 0) inv_ctr--; } if (inv_ctr == 50) inverted = TRUE; if(inv_ctr == 0) inverted = FALSE; } //--------------------------------------------------------------------------- int main(void) { UCHAR x,y,last_pos; UCHAR max_fwd_speed, max_rev_speed, low_fwd_speed, low_rev_speed; int16_t xx,yy; outp(BV(CS01), TCCR0); /* use CLK/8 source for counter */ outp(0x9f, PORTD); /* turn all leds off */ outp(0xff, DDRD); /* port D all outputs */ outp(0x00,PORTB); /* port B all bits off */ outp(0x3f,DDRB); /* low 6 bits outputs, motor control */ outp(0x00,DDRA); // 8 bits analog inputs outp(0,PORTA); // Clear port A outp(0x85,ADCSR); // Enable A to D converter with /32 conv clk. ( 230.4 khz) outp(0x1f,PORTC); // port C bits 0,1 have pullups outp(0xfc,DDRC); // bits 0,1 input on port C (inversion switches) outp(0x0b,TCCR2); // clear on compare, Prescale = 32 on T/C 2 (230400 hz) outp(12,OCR2); // T/C 2 compare register for 19200 hz interrupts sbi(TIMSK,OCIE2); // Enable timer 2 compare interrupts outp(47,UBRR); /* UART baud rate set to 9600 */ sbi(UCR,RXEN); // enable uart receiver sbi(UCR,RXCIE); // receiver interupt enabled x_offset = 0; y_offset = 0; inverted = FALSE; last_pos = inverted; rssi = 0; timer_servo_active = 50; timer_inhibit_flipper = 100; timer_fire_flipper = 0; minutes = 0; run_seconds = 0; txstate = 0; l_speed = 0; //Motors set to zero speed r_speed = 0; matrix0 = 0; matrix1 = 0; boost = 0; timer_led_green = 0; inversion_check(1); //Initialize inversion checker cbi(PORTC,BOOST); sbi(PORTD,LED_GREEN); //good packet indicator set to off - power save enabled testmode = 0; sei(); /* enable global interrupts */ do { if(run_seconds & 1) cbi(PORTD,LED_RESET); /* Blink yellow LED while no signal from controller*/ else sbi(PORTD,LED_RESET); } /* Remain here until signal received */ while(timer_led_green == 0); sbi(PORTD,LED_RESET); /* Reset complete, transmitter signal is being received, Yellow LED off*/ wdt_enable(WDTO_250MS); /* enable watchdog timer - 250 ms timeout. */ for(;;){ // ** MAIN LOOP starts here ** inversion_check(0); //Check our position. Set or clears "inverted" global variable. /* Get A-D converter values */ ad_pot = get_adc8(AD_POT); ad_bat_volt = get_adc8(3) ; ad_m_current = get_adc8(6) ; ad_rssi = get_adc8(7) ; /* Battery monitor not used in this bot */ /* if(ad_bat_volt < 97){ cbi(PORTD,LED_RED); //RED LED on if battery less than 19 volts }else{ sbi(PORTD,LED_RED); } */ if(txstate == 0) make_info_pkt(); //Transmit debug telemetry if(timer_led_green == 0) sbi(PORTD,LED_GREEN); //Clear the green DATA LED if timeout if(ad_rssi > 0x48) cbi(PORTD,LED_YELLOW); // DCD LED on (RF carrier detected) else sbi(PORTD,LED_YELLOW); y = (rx_pkt[RIGHTJOY_Y] >> 2) + 224; //Get joystick values, divide by 4 and convert to signed x = (rx_pkt[RIGHTJOY_X] >> 2) + 224; y = y - y_offset; x = x - x_offset; y = hard_limit(y,31); //Limit range to +/- 31 x = hard_limit(x,31); /* Do speed sensitive steering */ yy = (int8_t)y ; //Convert to 16 bit signed yy = abs(yy); //Absolute value of y (speed) 0..31 yy = 150 - (yy*3); //150 - (y * 3) Y speed range is now 150..57 xx = ((int8_t)x * yy) / 250; //Now modify the original x (steering) value with yy (modified speed). //XX Steering value ranges from 60% down to 23% of original x = (uint8_t)xx; //x varies depending on y. //Note that "boost" voltage is NOT ON during low speed turns //so 60% is really more like 90% of the max speed at the lower voltage. l_speed = hard_limit(y - x, 31); //Now mix the x (steering) with y (speed) r_speed = hard_limit(y + x, 31); /* Do dead zone logic +/- 3 */ #define deadplus 3 #define deadminus 0xfd if((l_speed > deadminus) && (l_speed <= 0xff)) l_speed = 0; //Create dead zones else if((l_speed > 0) && (l_speed <= deadplus)) l_speed = 0; if((r_speed > deadminus) && (r_speed <= 0xff)) r_speed = 0; else if((r_speed > 0) && (r_speed <= deadplus)) r_speed = 0; if(r_speed){ if(r_speed >= 128) r_speed += deadplus; //Remove dead zone offsets else r_speed -= deadplus; } if(l_speed){ if(l_speed >= 128) l_speed += deadplus; else l_speed -= deadplus; } /* Get controller switch matrix */ if(timer_led_green > 0){ //If valid RC data get the switch matrix bits matrix0 = rx_pkt[MATRIX0]; matrix1 = rx_pkt[MATRIX1]; }else{ matrix0 = 0; //Set to zero if RC signal invalid matrix1 = 0; } if(matrix1 & m_Z){ //If Z button pushed... y_offset = (rx_pkt[RIGHTJOY_Y] >> 2) + 224; //save the joystick neutral settings x_offset = (rx_pkt[RIGHTJOY_X] >> 2) + 224 ; } max_fwd_speed = MAX_FWD_SPEED; max_rev_speed = MAX_REV_SPEED; low_fwd_speed = QTR_FWD_SPEED; low_rev_speed = QTR_REV_SPEED; /* Now get the left motion controller bits */ /* These will override the joy stick control */ /* They control the autonomous modes */ if(matrix0 & m_south){ //Full speed reverse and point nose at any target l_speed = max_rev_speed; r_speed = max_rev_speed; if(bit_is_clear(PORTC,TARGET_LEFT_LED)){ /* Target to left */ r_speed = low_rev_speed; l_speed = max_rev_speed; } if(bit_is_clear(PORTC,TARGET_RIGHT_LED)){ /* Target to right */ l_speed = low_rev_speed; r_speed = max_rev_speed; } if(bit_is_clear(PORTC,TARGET_CENTER_LED)){ /* Target centered */ l_speed = max_rev_speed; r_speed = max_rev_speed; } l_speed = hard_limit(l_speed - x, 31); /* Mix in the x joystick value to allow some manual steering */ r_speed = hard_limit(r_speed + x, 31); } if(matrix0 & m_east){ //Spin clockwise until target acquired l_speed = max_fwd_speed; r_speed = max_rev_speed; if(bit_is_clear(PORTC,TARGET_LEFT_LED)){ /* Target to left */ r_speed = max_fwd_speed; l_speed = max_rev_speed; } if(bit_is_clear(PORTC,TARGET_RIGHT_LED)){ /* Target to right */ l_speed = max_fwd_speed; r_speed = max_rev_speed; } if(bit_is_clear(PORTC,TARGET_CENTER_LED)){ /* Target centered */ l_speed = 0; r_speed = 0; } } if(matrix0 & m_west){ //Spin counter clockwise until target acquired l_speed = max_rev_speed; r_speed = max_fwd_speed; if(bit_is_clear(PORTC,TARGET_LEFT_LED)){ /* Target to left */ r_speed = max_fwd_speed; l_speed = max_rev_speed; } if(bit_is_clear(PORTC,TARGET_RIGHT_LED)){ /* Target to right */ l_speed = max_fwd_speed; r_speed = max_rev_speed; } if(bit_is_clear(PORTC,TARGET_CENTER_LED)){ /* Target centered */ l_speed = 0; r_speed = 0; } } if(matrix0 & m_north){ //Full Speed ahead and steer towards any target l_speed = max_fwd_speed; r_speed = max_fwd_speed; if(bit_is_clear(PORTC,TARGET_LEFT_LED)){ /* Target to left */ r_speed = max_fwd_speed; l_speed = low_fwd_speed; } if(bit_is_clear(PORTC,TARGET_RIGHT_LED)){ /* Target to right */ l_speed = max_fwd_speed; r_speed = low_fwd_speed; } if(bit_is_clear(PORTC,TARGET_CENTER_LED)){ /* Target centered */ l_speed = max_fwd_speed; r_speed = max_fwd_speed; } l_speed = hard_limit(l_speed - x, 31); /* Mix in the x joystick value to allow some manual steering */ r_speed = hard_limit(r_speed + x, 31); } /* 100 times/sec we fire the left and right IR LEDs to update target location data in globals r_IR and l_IR */ if(timer_ir_led == 0){ timer_ir_led = 96; //10ms timeout value process_IR(); //Get info from IR object detector } opponent_detect(); adjust_motor_speed(); if (((matrix0 & m_left_fire) == 0) && (last_matrix0 & m_left_fire)){ timer_inhibit_flipper = 75; //Inhibit re-firing for 750ms } x = 0; /* Servo not used in this bot */ /* if(matrix1 & m_right_fire){ //Raise flipper with servo timer_servo_active = 25; //250 ms timeout if(inverted) x = 7; else x = 23; } else{ if(inverted) x = 23; else x = 7; } if(inverted != last_pos){ //We've been flipped last_pos = inverted; timer_servo_active = 50; //500ms. Allow servo to reverse sides } if(timer_servo_active) servo_pulse_width = x; else servo_pulse_width = 0; */ last_matrix0 = matrix0; last_matrix1 = matrix1; } } //--------------------------------------- void delay_100us(uint8_t count) { while(count){ outp(256 - ((F_CPU/80000) -1), TCNT0); outp(BV(TOV0), TIFR); while(!(inp(TIFR) & BV(TOV0))); count--; } } void delay_10ms(uint8_t count) { while(count){ delay_100us(100); count--; } } //---------------------------------------------------------------------- int integrate(UCHAR speed, UCHAR mfb, int integ){ UCHAR i,tc; speed = speed + 128; //convert speed to unsigned if (speed > mfb) tc = speed - mfb; else tc = mfb - speed; tc = tc >> 2; if(tc == 0) tc = 1; for(i=0;i mfb) { integ++; if (integ > 127) integ = 127; //Clip at +127 } if(speed < mfb){ integ--; if (integ < (-128)) integ = -128; //Clip at -128 } } return integ ; } //---------------------------------------------------------------------- /* Adjust motor speeds. Cause feedback values to match speed control input values. If bot is inverted right and left are exchanged and fwd/rev speeds swapped. */ void adjust_motor_speed(){ UCHAR i; static int r_integrate, l_integrate; l_speed = mpyx125(l_speed); //mpy X 1.25 r_speed = mpyx125(r_speed); /* l_speed = mpyx125(l_speed); //Again for an effective X 1.56 r_speed = mpyx125(r_speed); */ if(inverted){ //Swap left and right and fwd/rev when inverted i = l_speed; //Exchange left and right l_speed = r_speed; r_speed = i; r_speed = (r_speed ^ 0xff) + 1 ; //2s compliment makes it negative l_speed = (l_speed ^ 0xff) + 1 ; } if((l_speed != 0) || (r_speed != 0)) timer_mstop = 20; //200ms breaking when speed goes to zero if((l_speed == 0) && (r_speed == 0) && (timer_mstop == 0)){ r_integrate = 0; l_integrate = 0; return; } rmfb = get_adc8(AD_RM_SPEED) ; //get right motor feedback (range is +/- 26 decimal around 128 center) lmfb = get_adc8(AD_LM_SPEED) ; //get left motor feedback //Adjust motor speeds based on feedback values //Note: right_speed and left_speed must range between -128 and +127 //to use the full pwm speed range. r_integrate = integrate(r_speed,rmfb,r_integrate); l_integrate = integrate(l_speed,lmfb,l_integrate); right_speed = r_integrate; left_speed = l_integrate; } //----------------------------------------------------------------------- UCHAR speed_to_pulse_rate(int speed) { if (speed < 0) { //See if speed is negative ! return (UCHAR)(speed & 0x7f); } else { return (UCHAR)((speed ^ 0x7f) & 0x7f) ; } } //----------------------------------------------------------------------- /* Come here 19200 times per second. Executes in about 20uS*/ SIGNAL(SIG_OUTPUT_COMPARE2) { char x,vb; /* if(timer_servo > 0){ sbi(PORTC,SERVO) ; timer_servo--; } else { cbi(PORTC,SERVO); } */ if(--prescale == 0){ //This executes 100 times per sec. // prescale = 96; prescale = 192; if(prescale1sec > 0) prescale1sec--; if(timer_led_green > 0) timer_led_green--; // if(timer_inhibit_flipper > 0) timer_inhibit_flipper--; if(timer_mstop > 0) timer_mstop--; // if(timer_servo == 0) timer_servo = servo_pulse_width; //Servo pulse width walue is 104uS intervals // if(timer_servo_active > 0) timer_servo_active--; } if(prescale1sec == 0){ //This executes once per second prescale1sec = 100; if(prescaleMinute > 0) prescaleMinute--; run_seconds++; } if(prescaleMinute == 0){ //This executes once per minute prescaleMinute = 60; minutes++; } /* Motor control code */ outp(workB,PORTB); //Output to the H-Bridge workB = 0; /* adjust right pulse rate modulator */ if (right_prm < P_width) { workB = workB | BV(RM_ENABLE); //L293D Enable if (dir_flag_right == FWD) workB |= BV(FwdRight); //Set FwdRight bit else workB |= BV(RevRight); //else set RevRight bit } if (right_prm-- == 0) { right_prm = speed_to_pulse_rate(right_speed) + P_width -1; if (right_speed < 0) dir_flag_right = REV; else dir_flag_right = FWD; } /*adjust left pulse rate modulator */ if (left_prm < P_width) { workB = workB | BV(LM_ENABLE); //L293D Enable if (dir_flag_left == FWD) workB = workB | BV(FwdLeft) ; else workB = workB | BV(RevLeft); } if (left_prm-- == 0) { left_prm = speed_to_pulse_rate(left_speed) + P_width -1; if (left_speed < 0) dir_flag_left = REV; else dir_flag_left = FWD; } /* Motor current is measured as 0.5 volts per amp. ADC: 255 = 5 volts */ if (ad_m_current > 25){ workB = 0; // Limit total motor current to 1000 ma (500 ma each) } #define SLOW 18 vb = 1; //Assume Voltage Boost on if(ad_m_current > 15) vb = 0 ; //No boost if current > 600 ma x = (l_speed ^ r_speed) & 0x80; if((x) && !(matrix0 & (m_east | m_west))) vb = 0; //No boost if left and right speeds are in opposite directions (bot rotating) //Execption - Boost remains on if east or west buttons pushed if((abs((char)l_speed) < SLOW) || (abs((char)r_speed) < SLOW)) vb = 0; if((vb) && (ad_m_current < 10)) sbi(PORTC,BOOST); else cbi(PORTC,BOOST); if ((l_speed == 0) && (r_speed == 0) && (timer_mstop == 0)) workB = 0; if(testmode == 1) workB = 0; if(timer_led_green == 0) workB = 0; //Stop the bot if no control data /* End interrupt and motor control code */ } //----------------------------------------------------------------------- /* UART receive interrupt handler */ SIGNAL(SIG_UART_RECV) { static UCHAR rxstate = 0; UCHAR ch,i; ch = inp(UDR); //Get the current rx char switch (rxstate){ case 0: if((ch == SYNC2) && (last_rx == SYNC1)){ rxstate = 1; } break; case 1: rx_buf[ADDRESS] = ch; rxstate = 2; break; case 2: rx_buf[COUNT] = ch; rxstate = 3; rx_ptr = DATA; rx_ctr = ch + 2; if((rx_ctr + 4) > RXSIZE) rxstate = 0; //Quit if buffer would overflow break; case 3: rx_buf[rx_ptr++] = ch; rx_ctr--; if(rx_ctr == 0){ if((ck_rx_crc() == 0) && (rx_buf[ADDRESS] == BOT_ADDR)){ //Good packet received for us for(i=0; i < (rx_buf[COUNT] + 2); i++) rx_pkt[i] = rx_buf[i]; //Copy to packet buffer cbi(PORTD,LED_GREEN); // Light up the data LED timer_led_green = 5; //50ms timeout wdt_reset() ; //Reset Watchdog timer } rxstate = 0; } break; } last_rx = ch; //Save last char } //----------------------------------------------------------------------- /* Build a packet and send debug data */ void make_info_pkt(){ uint8_t i; i = ADDRESS; tx_buf[i] = 1; // address of monitor i = DATA; tx_buf[i++] = 0x01; //data type = 1 tx_buf[i++] = l_speed; //1 Left motor speed tx_buf[i++] = r_speed; //2 right Motor speed tx_buf[i++] = ad_m_current; //3 Motor current tx_buf[i++] = ad_bat_volt; //4 Battery voltage/10 tx_buf[i++] = ad_rssi; //5 recv sig strength tx_buf[i++] = ir_sens_left; //6 left ir sensor tx_buf[i++] = ir_sens_right; //7 right ir sensor tx_buf[i++] = matrix0; //8 controller switch matrix byte 0 tx_buf[i++] = matrix1; //9 controller switch matrix byte 1 tx_buf[i++] = run_seconds; //10 run time in seconds tx_buf[i++] = lmfb; tx_buf[i++] = rmfb; tx_buf[COUNT] = i - DATA; //Set byte count field to data length set_tx_buffer_crc(); //compute and add CRC16 bits to end of pkt txstate = 2; //Tell scheduler it's ready to transmit //outp(0x28,UCR); //enable UART TX and interrupt sbi(UCR, TXEN); sbi(UCR, UDRIE); outp(0xff,UDR); // Send a FF to get things started } //---------------------------------------------------------------------- /* UART Transmit interrupt handler */ SIGNAL(SIG_UART_DATA) { switch(txstate) { case 0: break; case 1: case 2: case 3: outp(0xff,UDR) ; txstate++; break; case 4: outp(SYNC1,UDR); txstate++; break; case 5: outp(SYNC2,UDR); txstate++; break; case 6: tx_ptr = 0; tx_ctr = tx_buf[COUNT] + 4; txstate++; //fall through... case 7: outp(tx_buf[tx_ptr++],UDR); //Now send buffer contents tx_ctr--; if (tx_ctr == 0){ txstate = 8; } break; case 8: txstate = 0; outp(0xff,UDR); //Feed the uart data register one last time to clear this interrupt cbi(UCR,UDRIE); //Stop transmitter and interrupts cbi(UCR,TXEN); break; } } //--------------------------------------------------------------------------- /* end of file */