/* Invertabot combat robot control program, May 26 2003 Copyright 2003 by Dale Alan Heatherington Home page: http://www.wa4dsy.net/robot/invertabot/ Atmel AVR AT90S8535 microcontroller Compiled with avr-gcc on SuSE Linux 8.1 (See Makefile) Based on T-Zero control pgm and some ported PIC code from Delta Force mini-sumo bot. */ /* 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 #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 //Sets time in 10ms intervals for inversion detection logic #define INVTIME 5 /* Packet format from R/C controller: 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 SERVO 5 /* PWM output to servo that lifts the flipper over seams */ #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_BLUE 7 /* low true, POWER ON */ #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 hacked Wingman game 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 ----- */ /* Motor "BACK EMF" feedback ranges are set in fixed hardware by resistors in the OP-AMP motor speed feedback circuit. Ranges will vary depending on the battery voltage. The same circuit and code can't be used with different voltages. Since there is no way to tweek and adjust this, the code has several kludges to adjust the range to match the Joy-Stick range. Future hardware should have pots to set the range so the software code can be the same for all voltages. ie: changing the battery voltage shouldn't require changing this code. */ /* Port B motor and flipper control bits */ #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 FLIPPER 4 /* Flipper weapon firing bit (high true) */ #define P_width 6 /*Motor pulse width in interrupt ticks (104 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; volatile UCHAR timer_servo_active; UCHAR ad_pot; int ii; UCHAR prescale ; UCHAR prescale100; UCHAR prescale1sec; UCHAR prescaleMinute; UCHAR timer10ms ; UCHAR timer1sec; 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 chance 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; } //--------------------------------------------------------------------------- /****** MAIN HERE ******/ //--------------------------------------------------------------------------- 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; UCHAR servo_setting; 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(0x1f,DDRB); /* low 5 bits outputs, motor and flipper 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(24,OCR2); // T/C 2 compare register for 9600 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; sbi(PORTD,LED_GREEN); //good packet indicator set to off inversion_check(1); //Initialize inversion checker //bot_mode = MODE_STOP; testmode = 0; cbi(PORTD,LED_BLUE); /* Power ON indicator */ sei(); /* enable global interrupts */ do { if(run_seconds & 1) cbi(PORTD,LED_BLUE); /* Blink blue LED while no signal from controller*/ else sbi(PORTD,LED_BLUE); } /* Remain here until signal received */ while(timer_led_green == 0); /* Reset complete, transmitter signal is being received */ wdt_enable(WDTO_250MS); /* enable watchdog timer - 250 ms timeout. */ cbi(PORTD,LED_BLUE); //Blue LED on steady /*---------------------------------------------------------*/ for(;;){ // ** MAIN LOOP starts here ** inversion_check(0); //Check our position /* 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) ; if(ad_bat_volt < 97){ cbi(PORTD,LED_RED); //RED LED on if battery less than 19 volts }else{ sbi(PORTD,LED_RED); } //servo_flag = 1; 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 = 100 - (yy*3); //100 - (y * 3) range is now 100..7 xx = ((int8_t)x * yy) / 100; //Now modify the original x value with yy. //Value ranges from 100% down to 7% of original x = (uint8_t)xx; //x varies depending on y. l_speed = hard_limit(y - x, 31); r_speed = hard_limit(y + x, 31); /* Do dead zone logic +/- 4 */ #define deadplus 4 #define deadminus 0xfc 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; } l_speed = mpyx125(l_speed); //Mpy by 1.25 r_speed = mpyx125(r_speed); // ..also this compensates for the range lost in the dead zone /* Get controller switch matrix */ //servo_flag = 1; 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;//servo_flag = 1; 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; } } //servo_flag = 1; 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) //If button is pushed... && ((last_matrix0 & m_left_fire) == 0)){ //..and was not pushed last time... //Then fire weapon timer_fire_flipper = 288; //30 ms } if (((matrix0 & m_left_fire) == 0) && (last_matrix0 & m_left_fire)){ timer_inhibit_flipper = 75; //Inhibit re-firing for 750ms } if(inverted) servo_setting = 23; else servo_setting = 7; if(matrix1 & m_right_fire){ //Raise flipper with servo timer_servo_active = 150; //1500 ms timeout if(inverted) servo_setting = 7; else servo_setting = 23; } if(inverted != last_pos){ //We've been flipped last_pos = inverted; timer_servo_active = 150; //1500ms. Allow servo to reverse sides } servo_pulse_width = servo_setting; last_matrix0 = matrix0; last_matrix1 = matrix1; } /* END OF MAIN LOOP */ } //--------------------------------------- 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 = 10; //100ms 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) 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; } //----------------------------------------------------------------------- /* Convert a signed value to an absolute value between 127 and 0 0 becomes 127 127 becomes 0 -128 becomes 0 */ 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 9600 times per second and do various timers, motor PRM control, servo control and flipper control */ SIGNAL(SIG_OUTPUT_COMPARE2) { if(timer_ir_led >0) timer_ir_led--; if(timer_fire_flipper > 0) timer_fire_flipper--; if(timer_servo > 0){ sbi(PORTC,SERVO) ; timer_servo--; } else { cbi(PORTC,SERVO); } if(--prescale == 0){ //This executes 100 times per sec. prescale = 96; 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) { if(timer_servo_active > 0){ timer_servo = servo_pulse_width; //Servo pulse width value is 104uS intervals timer_servo_active--; } else timer_servo = 0; } } 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. Does "Pulse Rate Modulation". Pulse width is constant, frequency varies. It's not PWM. */ outp(workB,PORTB); //Output bits in workb to the H-Bridge workB = 0; //Clear all bits in workb /* adjust right pulse rate modulator */ if (right_prm < P_width) { 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) { 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; } /* Boost voltage control */ /* Motor current is measured as 0.1 volts per amp. ADC: 255 = 5 volts */ /* //WARNING: This code blows up the boost regulator chip! Why????? (Do not use) if(ad_m_current <= 20){ //When speed is high and current low ( <4 amps) turn on the boost voltage if(((speed_to_pulse_rate(left_speed) < P_width) //IF speed more than half throttle || (speed_to_pulse_rate(right_speed) < P_width)) &&(dir_flag_left == dir_flag_right)){ //And going fwd or rev, not rotating sbi(PORTC,BOOST); } //Then boost motor voltage else cbi(PORTC,BOOST); }else cbi(PORTC,BOOST); */ /* This less complex boost logic works fine */ if(ad_m_current > 30){ //greater than 6 amps , turn OFF voltage booster cbi(PORTC,BOOST); } if(ad_m_current <= 20){ // less than 4 amps, turn on voltage booster sbi(PORTC,BOOST); } /* Software current limiter */ /* Motor current is measured as 0.1 volts per amp. ADC: 255 = 5 volts */ if (ad_m_current >= 50){ workB = 0; // Limit total motor current to 10 amps . } 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 motor control code */ /* Flipper activation */ if((timer_fire_flipper > 0) && (timer_inhibit_flipper == 0)){ workB |= BV(FLIPPER); //Fire flipper weapon } } //----------------------------------------------------------------------- /* 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 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 */