/* * Firmware for ECEBot Zeta (Fall 2008 version) * * Rob Maher * * Copyright (c) 2005 Department of Electrical and Computer Engineering, Montana State University * * Audit trail * Start RCM 11 October 2005 Initial C program to handle motors, LEDs, display, and bumpers * * RCM 16 October 2005 Completed 7 modes, ready to test for firmware (version 2.0) * * RCM 15 September 2008 for CSMB12 board * * RCM 16 October 2008 serial experiments * * Description: * Includes, defines, and globals listed first, then * Interrupt service routines, * Low level display and control functions, * High level command functions, * and finally, * the main() function. */ /**********************************************************************************************/ /* * Modes are set by selecting DIP switch pattern (in RUN mode) and pressing Reset button. * NOTE that the DIP switches are numbered left to right! * NOTE that only switches 1-4 are connected to the microcontroller; switches 5-8 are unused. * * Mode 0 0000 PCB test (flashes LEDs 4&5, does 4-digit display pattern) * Mode 1 1000 Analog-to-Digital (Displays digital representation of potentiometer wiper position) * Mode 2 0100 Forward test (rolls forward for 5 seconds, then stops) * Mode 3 1100 Forward-Backward (rolls forward and then backward repeatedly) * Mode 4 0010 Dance pattern (rolls forward, then rotates, then forward again, repeatedly) * Mode 5 1010 Bumper Bot (rolls forward; if left bumper hits, backs up and turns right; if * right bumper hits, backs up and turns left) * Mode 6 0110 Programmable mode (user must download a command sequence to RAM) * Mode 7 1110 Figure-Eight (Robot executes a sequence of segments and turns to be a figure-8 pattern) * * Modes 8-15: not defined * */ /**********************************************************************************************/ #include /* common defines and macros */ #include /* derivative information */ #include "main_asm.h" /* interface to the assembly module */ #include #pragma LINK_INFO DERIVATIVE "mc9s12c128" #define CODE_REV_MAJOR 3 /* Revision code */ #define CODE_REV_MINOR 1 //RCM, was 0 #define DELAY 2 /* Tick count (millisec) used for display updates */ #define MAX_STEP 256 /* Number of 2-byte commands available for mode 6 downloads */ #define MAX_TIME 0x9f60 /* Pulse width corresponding to maximum "on" time */ #define MIN_TIME 0x79E0 /* Pulse width to motor corresponding to minimum "on" time */ /* 0x79E0 for 1.3ms; 0x9f60 for 1.7ms 0x8CA0 for 1.5ms */ #define RIGHT_FORWARD MIN_TIME #define LEFT_FORWARD MAX_TIME #define RIGHT_BACKWARD MAX_TIME #define LEFT_BACKWARD MIN_TIME /* macro to adjust tick count to compensate 1ms vs. 1.024 ms (rounds to nearest) */ #define MS(a) ((int)(0.5+a/1.024)) /* enable LED4 (left, PTAD6) and LED5 (right, PTAD7) */ #define LEFT_LED_ON(a) ((a)?(PTAD_PTAD6 = 0):(PTAD_PTAD6 = 1)) /* LED4 (active low) */ #define RIGHT_LED_ON(a) ((a)?(PTAD_PTAD7 = 0):(PTAD_PTAD7 = 1)) /* LED5 (active low) */ /* enable interrupt on timer chan 0 (right motor PWM) and timer chan 1 (left motor PWM) */ #define MOTOR_INTERRUPTS_ON(a) ((a)?(TIE_C0I = 1,TIE_C1I = 1):(TIE_C0I = 0,TIE_C1I = 0)) /**********************************************************************************************/ /* * Global variables */ volatile int Val_right,Val_left,Speed_right,Speed_left; volatile int Vcnt; unsigned int Bunch_o_mem[MAX_STEP] @(0x1000-MAX_STEP*sizeof(unsigned int)); // pre-test: unsigned int Bunch_o_mem[MAX_STEP] ; int Step_num; volatile unsigned char Bump=0x03; void (*Rti_func)(void) = NULL; void initsci(void); void putchar(char); char getchar(void); char scichar_ready(void); /**********************************************************************************************/ /* * Interrupt service routines */ /**********************************************************************************************/ void interrupt 7 RTI_handler() { /* * This function is called when a real time interrupt (RTI) occurs. * * The function asserts the RTI flag and increments global variable Vcnt. * If a function pointer has been assigned to global variable Rti_func, then * that function is also called. */ CRGFLG_RTIF = 1; // Set RTI flag to reset counter (MUST do this each time) Vcnt++; if( Rti_func ) (Rti_func)(); } /* end of RTI_handler() */ /**********************************************************************************************/ void interrupt 8 outcap_0(void) { /* * Pulse width modulation (PWM) control interrupt. * This interrupt is for the right motor */ TFLG1 = 0x01; // Clear chan 0 flag TC0 = TC0 + Speed_right; // Set time for next interrupt (current time + offset) Val_right = (Val_right+1)%9; // Variable counts 0-8: when zero, pulse out is high, otherwise low (1 out of 9 duty cycle) if( Val_right ) TCTL2 = (TCTL2&0xFC)|0x02 ; // Clear output on next interrupt else TCTL2=(TCTL2&0xFC)|0x03; // Set output on next interrupt } /* end of outcap_0() */ /**********************************************************************************************/ void interrupt 9 outcap_1(void) { /* * Pulse width modulation (PWM) control interrupt. * This interrupt is for the left motor */ TFLG1 = 0x02; // Clear chan 1 flag TC1 = TC1 + Speed_left; // Set time for next interrupt (current time + offset) Val_left = (Val_left+1)%9; // Variable counts 0-8: when zero, pulse out is high, otherwise low (1 out of 9 duty cycle) if( Val_left ) TCTL2 = (TCTL2&0xF3) | 0x08 ; // Clear output on next interrupt else TCTL2=(TCTL2&0xF3) | 0x0C; // Set output on next interrupt } /* end of outcap_1() */ /**********************************************************************************************/ void delay_ms(int delcount) { /* * Delay for specified number of milliseconds. * The function also checks the bumper status: if bump, exit and save it in global variable 'Bump'. * Millisecond timing is based on Real Time Interrupt, and may be inaccurate (1.024 ms vs 1.000 ms); SEE the * macro MS( ) that will round a constant to the nearest 1.024ms ticks. */ int vstop; vstop=Vcnt+delcount; // add the specified number of millisecond ticks to Vcnt (current count)... while(Vcnt != vstop ){ Bump = (PTAD&0x03); // Stay here and look for any bump contacts if(Bump!=0x03) break; } // ... and then wait until count expires. }/* end delay_ms() */ /**********************************************************************************************/ void delay_ms_abs(int delcount) { /* * Delay for specified number of milliseconds. * The function DOES NOT check the bumper status. * Millisecond timing is based on Real Time Interrupt, and may be inaccurate (1.024 ms vs 1.000 ms); SEE the * macro MS( ) that will round a constant to the nearest 1.024ms ticks. */ int vstop; vstop=Vcnt+delcount; // add the specified number of millisecond ticks to Vcnt (current count)... while(Vcnt != vstop ){ } // ... and then wait until count expires. }/* end delay_ms_abs() */ /**********************************************************************************************/ unsigned char read_switches(void){ /* Function reads the DIP switches * and returns the byte pattern. * NOTE that only the upper 4 bits are connected to DIP 1-4, * and also note that 'on' switch is a 0 (active low) */ unsigned char hold_DDRT,switch_pattern; hold_DDRT = DDRT; // Save T data direction PTM=0x0F; // Make sure display transistors are off DDRT=0x00; // Set T to be inputs delay_ms_abs(2); switch_pattern=PTT; // read port T switch_pattern=PTT; // redundant port T read DDRT=hold_DDRT; // restore DDRT delay_ms_abs(2); return(switch_pattern); }/* end of read_switches() */ /**********************************************************************************************/ void display(int digit,unsigned char four_bit_code, unsigned char decimal) { /* * This function selects which digit to display and which segments * to turn on. Parameter decimal is a boolean: zero means no decimal * point, non-zero means display the decimal point. */ switch(digit) { case 0: PTM_PTM3 = 0; PTM_PTM4 = 0; PTM_PTM5 = 0; break; case 1: //PTM=0x20; PTM_PTM3 = 0; PTM_PTM4 = 0; PTM_PTM5 = 1; break; case 2: //PTM=0x10; PTM_PTM3 = 0; PTM_PTM4 = 1; PTM_PTM5 = 0; break; case 3: //PTM=0x30; PTM_PTM3 = 0; PTM_PTM4 = 1; PTM_PTM5 = 1; break; default: //PTM=0x08; // off PTM_PTM3 = 1; PTM_PTM4 = 0; PTM_PTM5 = 0; break; } PTT= (four_bit_code<<4); PTM_PTM2 = (decimal)?0:1; }/* end of display() */ /**********************************************************************************************/ void display_decimal(unsigned int val,int decimal_position){ /* * This function takes an input val and displays a 4-digit * decimal version on the robot display. Only the least signifcant * 4 digits are displayed. The parameter decimal_position specifies * where the decimal point is located: * <0 is no decimal point and leading zero blanking * 0 is rightmost (and leading zero blanking) * 1 is one decimal place (and blanking) * 2 is two " " * 3 is three decimal place * >=4 is no decimal point but NO zero blanking */ int temp,blank; temp=(val%10000)/1000; if( temp == 0 && decimal_position<3){ blank=1; temp=0x0F; } else blank=0; if(decimal_position==3) display(3,(unsigned char)temp,(unsigned char)1); else display(3,(unsigned char)temp,(unsigned char)0); delay_ms_abs(DELAY); temp= (val%1000)/100; if( temp == 0 && blank==1 && decimal_position<2){ temp=0x0F; } else blank=0; if(decimal_position==2) display(2,(unsigned char)temp,(unsigned char)1); else display(2,(unsigned char)temp,(unsigned char)0); delay_ms_abs(DELAY); temp= (val%100)/10; if( temp == 0 && blank==1 && decimal_position < 1){ temp=0x0F; } else blank=0; if(decimal_position==1) display(1,(unsigned char)temp,(unsigned char)1); else display(1,(unsigned char)temp,(unsigned char)0); delay_ms_abs(DELAY); temp= val%10; if(decimal_position==0) display(0,(unsigned char)temp,(unsigned char)1); else display(0,(unsigned char)temp,(unsigned char)0); delay_ms_abs(DELAY); }/* end of display_decimal() */ /**********************************************************************************************/ void display_hexadecimal(unsigned int val,int decimal_position){ /* * This function takes an input val and displays a 4-digit * hexadecimal version on the robot display. The decoder shows symbols * instead of ABCDEF (refer to 74LS47 datasheet). Only the least signifcant * 4 digits are displayed. The parameter decimal_position specifies * where the decimal point is located: * <0 is no decimal point and leading zero blanking * 0 is rightmost (and leading zero blanking) * 1 is one decimal place (and blanking) * 2 is two " " * 3 is three decimal place * >=4 is no decimal point but NO zero blanking */ int temp,blank; temp= val/0x1000; if( temp == 0 && decimal_position<3){ blank=1; temp=0x0F; } else blank=0; if(decimal_position==3) display(3,(unsigned char)temp,(unsigned char)1); else display(3,(unsigned char)temp,(unsigned char)0); delay_ms_abs(DELAY); temp= (val%0x1000)/0x100; if( temp == 0 && blank==1 && decimal_position<2){ temp=0x0F; } else blank=0; if(decimal_position==2) display(2,(unsigned char)temp,(unsigned char)1); else display(2,(unsigned char)temp,(unsigned char)0); delay_ms_abs(DELAY); temp= (val%0x100)/0x10; if( temp == 0 && blank==1 && decimal_position < 1){ temp=0x0F; } else blank=0; if(decimal_position==1) display(1,(unsigned char)temp,(unsigned char)1); else display(1,(unsigned char)temp,(unsigned char)0); delay_ms_abs(DELAY); temp= val%0x10; if(decimal_position==0) display(0,(unsigned char)temp,(unsigned char)1); else display(0,(unsigned char)temp,(unsigned char)0); delay_ms_abs(DELAY); }/* end of display_hexadecimal() */ /**********************************************************************************************/ void display_off(void){ //PTM=0x08; // off PTM_PTM3 = 1; PTM_PTM4 = 0; PTM_PTM5 = 0; }/* end display_off() */ /**********************************************************************************************/ void flash_halt(void){ /* Flash the display every half second. */ int i; RIGHT_LED_ON(FALSE); LEFT_LED_ON(FALSE); for(;;){ for(i=0;i<500/(4*DELAY);i++){ display_decimal(0,-1); } display_off(); delay_ms_abs(500); }/* flash forever */ }/* end flash_halt() */ /**********************************************************************************************/ void AtoD(void){ /* * A/D for potentiometer */ int pot; for(;;){ ATDCTL5 = 0x04; while( (ATDSTAT0 & 0x80)==0 ){ }; pot=ATDDR0H; display_decimal(pot,0); } }/* end AtoD() */ /**********************************************************************************************/ void board_test(void){ /* Test program * */ unsigned char num; int i,j; /* * Test of SW2 on module: PP1 (port p bit 1) */ RIGHT_LED_ON(FALSE); LEFT_LED_ON(TRUE); for(;;) { for(num=0;num<16;++num){ PTAD= PTAD ^ 0xC0; /* toggle LED4 and LED5 */ for(i=0;i<30;i++){ for(j=0;j<4;++j){ /* show display in all 4 digits */ display(j,num,(unsigned char)(num%2)); delay_ms_abs(DELAY); } } } for(j=0;j<4;++j){ for(num=0;num<16;++num){ PTAD= PTAD ^ 0xC0; /* toggle LED4 and LED5 */ for(i=0;i<60;i++){ display(j,num,(unsigned char)(num%2)); /* test each digit separately */ delay_ms_abs(DELAY); } /* end i */ } /* end num */ }/* end j */ // do{ // }while ( !scichar_ready() ); }/* end forever loop */ }/* end of board_test() */ /**********************************************************************************************/ void forward_ms(int dur){ /* * Engage motors for forward motion of 'dur' milliseconds */ Speed_right = RIGHT_FORWARD; Speed_left = LEFT_FORWARD; MOTOR_INTERRUPTS_ON(TRUE); LEFT_LED_ON(FALSE); RIGHT_LED_ON(FALSE); delay_ms(dur); MOTOR_INTERRUPTS_ON(FALSE); }/* end forward_ms() */ /**********************************************************************************************/ void right_ms(int dur){ /* * Engage motors for right rotation of 'dur' milliseconds */ Speed_right = RIGHT_BACKWARD; Speed_left = LEFT_FORWARD; MOTOR_INTERRUPTS_ON(TRUE); LEFT_LED_ON(FALSE); RIGHT_LED_ON(TRUE); delay_ms(dur); MOTOR_INTERRUPTS_ON(FALSE); }/* end right_ms() */ /**********************************************************************************************/ void left_ms(int dur){ /* * Engage motors for left rotation of 'dur' milliseconds */ Speed_right = RIGHT_FORWARD; Speed_left = LEFT_BACKWARD; MOTOR_INTERRUPTS_ON(TRUE); LEFT_LED_ON(TRUE); RIGHT_LED_ON(FALSE); delay_ms(dur); MOTOR_INTERRUPTS_ON(FALSE); }/* end left_ms() */ /**********************************************************************************************/ void backward_ms(int dur){ /* * Engage motors for backward motion of 'dur' milliseconds */ Speed_right = RIGHT_BACKWARD; Speed_left = LEFT_BACKWARD; MOTOR_INTERRUPTS_ON(TRUE); LEFT_LED_ON(TRUE); RIGHT_LED_ON(TRUE); delay_ms(dur); MOTOR_INTERRUPTS_ON(FALSE); }/* end backward_ms() */ /**********************************************************************************************/ void five_seconds_forward(void){ /* * Turns motors on and runs forward for 5 seconds, then stops. Stop if bump (falls through forward_ms() ). * */ /* Do a countdown on the display each second. */ display(0,5,0); forward_ms(MS(1000)); display(0,4,0); forward_ms(MS(1000)); display(0,3,0); forward_ms(MS(1000)); display(0,2,0); forward_ms(MS(1000)); display(0,1,0); forward_ms(MS(1000)); display(0,0,0); flash_halt(); }/* end five seconds forward */ /**********************************************************************************************/ void forward_backward(void){ /* * Run alternately forward 2 seconds and backward 2 seconds, repeatedly. Stop if bump. */ for(;;){ forward_ms(MS(2000)); if(Bump != 0x03) break; delay_ms(200); // avoid abrupt direction change if(Bump != 0x03) break; backward_ms(MS(2000)); if(Bump != 0x03) break; delay_ms(200); // avoid abrupt direction change if(Bump != 0x03) break; }/* loop until bump */ flash_halt(); }/* end forward backward */ /**********************************************************************************************/ void dance(void){ /* * Run alternately forward 2 seconds and rotate 2 seconds, repeatedly. Stop if bump. */ for(;;){ forward_ms(MS(2000)); // forward if(Bump != 0x03) break; left_ms(MS(2000)); // rotate if(Bump != 0x03) break; }/* loop until bump */ flash_halt(); }/* end dance */ /**********************************************************************************************/ void bumper_bot(void){ /* * Run forward until switch bump. * If left switch hit, back up and turn right. * If right switch hit, back up and turn left. */ volatile int bump; int i; Speed_right= RIGHT_FORWARD; Speed_left= LEFT_FORWARD; MOTOR_INTERRUPTS_ON(TRUE); // Start rollin' ! for(;;){ do{ bump=PTAD&0x03; // Stay here and look for any bump contacts }while( bump==0x03 ); MOTOR_INTERRUPTS_ON(FALSE); // Stop motors on bump if( bump == 1){ // hit right bumper RIGHT_LED_ON(TRUE); LEFT_LED_ON(FALSE); } else{ // hit left bumper RIGHT_LED_ON(FALSE); LEFT_LED_ON(TRUE); } /* illuminate the display for just 50ms */ for(i=0;i<50/(4*DELAY);i++){ display_decimal(8888,-1); } RIGHT_LED_ON(FALSE); LEFT_LED_ON(FALSE); display_off(); /* Now back up for 2 seconds */ Speed_right= RIGHT_BACKWARD; Speed_left= LEFT_BACKWARD; MOTOR_INTERRUPTS_ON(TRUE); delay_ms_abs(MS(2000)); if( bump == 1 ){ //hit right switch, so turn left RIGHT_LED_ON(FALSE); LEFT_LED_ON(TRUE); Speed_right= RIGHT_FORWARD; Speed_left= LEFT_BACKWARD; } else{ //hit left switch, so turn right RIGHT_LED_ON(TRUE); LEFT_LED_ON(FALSE); Speed_right= RIGHT_BACKWARD; Speed_left=LEFT_FORWARD; } delay_ms(MS(675)); // execute the turn RIGHT_LED_ON(FALSE); LEFT_LED_ON(FALSE); Speed_right= RIGHT_FORWARD; // now return to forward direction Speed_left= LEFT_FORWARD; } /* end for() */ }/* end bumper_bot */ /**********************************************************************************************/ void prog_handler(void){ /* * This function is installed as a subroutine called from RTI_handler(). It is called * at the real time interrupt rate, currently every 1.024ms. * * The function reads the 2-byte commands from the Bunch_o_mem array, decodes the command, * and sets up the motor and LED parameters accordingly. * * The function "uninstalls itself" if it encounters a zero 2-byte command or if it encounters * a HALT command in the user RAM buffer (Bunch_o_mem). */ static int term_count,first_time=1; int command,duration; if( first_time ){ term_count=Vcnt; first_time=0; } if( term_count != Vcnt ) return; // Previous command still going, so continue if( Step_num >= MAX_STEP){ MOTOR_INTERRUPTS_ON(FALSE); // Ran out of command memory Rti_func = NULL; return; } Step_num++; /* * Command code is upper 3 bits. * Duration in milliseconds is lower 13 bits. * * The command encoding is: * 15 14 13 12 11 10 9 8 7 6 5 4 3 2 1 0 * | | | | | | | | | | | | | | | | * ----- ----------------------------------- * CODE DURATION (milliseconds) * * 000 - go forward * 001 - turn left * 010 - turn right * 011 - go backward * 100 - sleep (no motion) * 101 - " * 110 - " * 111 - HALT (stop moving and exit) * * Duration maximum: 2^13 -1 = 8191 ticks (1 tick = 1.024 milliseconds) ~= 8.39 seconds * Longer durations can be simulated by repeating the same control code on subsequent commands. * */ command = (Bunch_o_mem[Step_num] & 0xE000)>> 13; duration = Bunch_o_mem[Step_num] & 0x1FFF; term_count = Vcnt + duration; if(Bunch_o_mem[Step_num] == 0){ // exit if a 2-byte zero was encountered MOTOR_INTERRUPTS_ON(FALSE); Rti_func = NULL; return; } switch(command){ case 0: // go forward Speed_right= RIGHT_FORWARD; Speed_left= LEFT_FORWARD; MOTOR_INTERRUPTS_ON(TRUE); RIGHT_LED_ON(FALSE); LEFT_LED_ON(FALSE); break; case 1: // turn left Speed_right= RIGHT_FORWARD; Speed_left= LEFT_BACKWARD; MOTOR_INTERRUPTS_ON(TRUE); RIGHT_LED_ON(FALSE); LEFT_LED_ON(TRUE); break; case 2: // turn right Speed_right= RIGHT_BACKWARD; Speed_left= LEFT_FORWARD; MOTOR_INTERRUPTS_ON(TRUE); RIGHT_LED_ON(TRUE); LEFT_LED_ON(FALSE); break; case 3: // go backward Speed_right= RIGHT_BACKWARD; Speed_left= LEFT_BACKWARD; MOTOR_INTERRUPTS_ON(TRUE); RIGHT_LED_ON(TRUE); LEFT_LED_ON(TRUE); break; case 4: // sleep (note that 5 and 6 could be used for other command types) case 5: case 6: Speed_right= RIGHT_FORWARD; Speed_left= LEFT_FORWARD; MOTOR_INTERRUPTS_ON(FALSE); RIGHT_LED_ON(FALSE); LEFT_LED_ON(FALSE); break; case 7: // HALT default: MOTOR_INTERRUPTS_ON(FALSE); Rti_func = NULL; RIGHT_LED_ON(FALSE); LEFT_LED_ON(FALSE); break; }/* end switch */ } /* end prog_handler() */ /**********************************************************************************************/ void prog_bot(void){ /* * This function sets up the user programmable command sequence. * The user must load the sequence of 2-byte encoded commands to * processor RAM in the 'Bunch_o_mem' location (currently 0x0E00-0x0FFF). * * The command encoding is: * 15 14 13 12 11 10 9 8 7 6 5 4 3 2 1 0 * | | | | | | | | | | | | | | | | * ----- ----------------------------------- * CODE DURATION (milliseconds) * * 000 - go forward * 001 - turn left * 010 - turn right * 011 - go backward * 100 - sleep (no motion) * 101 - " * 110 - " * 111 - HALT (stop moving and exit) * * Duration maximum: 2^13 -1 = 8191 ticks (1 tick = 1.024 milliseconds) ~= 8.39 seconds * Longer durations can be simulated by repeating the same control code on subsequent commands. * * The setup installs a special function that is called from within the RTI_handler() interrupt * service routine: the global function pointer Rti_func is assigned prog_handler(). * * This function goes into a loop to display the current step in the command sequence, and monitors * the bumper switches for contact. If the bumper switches close, the robot sequence stops. */ int i; volatile char sw2; volatile unsigned char cmd, adr_hi, adr_lo, count; volatile char *bom; sw2=1; /* Display the RAM hexadecimal address for 3 seconds. Note that the display characters for * ABCDEF are the 74LS47 display codes, not the letters. * * IF user presses SW2 on the microprocessor module during the addr display, * wait for the serial command download sequence. * Otherwise proceed to excecute commands. */ for(i=0;i<3000/(4*DELAY);i++){ display_hexadecimal((unsigned int) Bunch_o_mem,4); if( (PTIP&0x02) == 0 ){ // watch for SW2 press! sw2=0; break; } } if( sw2 == 0){ // SW2 was pressed, so wait for data download /* * 0D (cr) * A8 (byte download) * 3E (dest adr hi) * 00 (dest adr lo) * FF (count) * xx byte * xx byte * xx byte * ... */ bom = (char *) Bunch_o_mem; cmd=getchar(); // CR cmd=getchar(); // A8 if(cmd != (unsigned char) 0xA8 ) goto ERR_HALT; adr_hi=getchar(); adr_lo=getchar(); count=getchar(); for(i=0;i<= (int) count;i++){ bom[i]=getchar(); } /* Sequence download complete: flash "1111". * User should now reset (and not press SW2) for command sequence execution. */ for(;;){ // TRAP here for(i=0;i<500/(4*DELAY);i++){ display_decimal(1111,-1); } display_off(); delay_ms_abs(500); } }// end SW2 check Step_num = -1; Rti_func = prog_handler ; // Install the pointer to the function called from within RTI_handler() while( Rti_func != NULL){ display_decimal(Step_num,-1); // Display the step number within the user's program sequence if( ((PTAD&0x03) != 0x03) ){ // check bump MOTOR_INTERRUPTS_ON(FALSE); // If bump, shut down motors and halt. Rti_func = NULL; for(;;){ RIGHT_LED_ON(FALSE); LEFT_LED_ON(FALSE); delay_ms_abs(500); RIGHT_LED_ON(TRUE); LEFT_LED_ON(TRUE); delay_ms_abs(150); } // trap here forever... } }/* end while */ /* * User 'Halt' command encountered, so just flash final step number (on and off) */ ERR_HALT: for(;;){ for(i=0;i<500/(4*DELAY);i++){ display_decimal(Step_num,-1); } display_off(); delay_ms_abs(500); } }/* end prog_bot */ /**********************************************************************************************/ void figure_eight(void){ /* * Program to attempt a figure-8 pattern. Robot starts in the lower left corner, proceeds up, * across middle, up to top, across top to upper left, then goes down, across middle, down, and * across bottom back to the starting point; then repeats. */ for(;;){ //forward 4 LL forward_ms(4000); if(Bump != 0x03) break; //right mid right_ms(675); if(Bump != 0x03) break; //forward 4 mid forward_ms(4000); if(Bump != 0x03) break; //left UR left_ms(675); if(Bump != 0x03) break; //forward 4 UR forward_ms(4000); if(Bump != 0x03) break; //left TOP left_ms(675); if(Bump != 0x03) break; //forward 4 TOP forward_ms(4000); if(Bump != 0x03) break; //left UL left_ms(675); if(Bump != 0x03) break; //forward 4 UL forward_ms(4000); if(Bump != 0x03) break; //left mid left_ms(675); if(Bump != 0x03) break; //forward 4 mid forward_ms(4000); if(Bump != 0x03) break; //right LR right_ms(675); if(Bump != 0x03) break; //forward 4 LR forward_ms(4000); if(Bump != 0x03) break; //right bot right_ms(675); if(Bump != 0x03) break; //forward 4 bot forward_ms(4000); if(Bump != 0x03) break; //right LL reorient right_ms(675); if(Bump != 0x03) break; // repeat! } /* loop until bump */ flash_halt(); }/* end figure_eight() */ /**********************************************************************************************/ /**********************************************************************************************/ /* * START OF MAIN() * */ /**********************************************************************************************/ void main(void) { volatile int i; //volatile int num,dispcnt; volatile unsigned char pat; //unsigned char bump; /* preset test Bunch_o_mem[0]= 0x0000 | 1191 ; //forward Bunch_o_mem[1]= 0x2000 | 1191 ; //left Bunch_o_mem[2]= 0x4000 | 1191 ; //right Bunch_o_mem[3]= 0x6000 | 1191 ; //back Bunch_o_mem[4]= 0x8000 | 1191 ; //sleep Bunch_o_mem[5]= 0xA000 | 1191 ; //sleep Bunch_o_mem[6]= 0xC000 | 1191 ; //sleep Bunch_o_mem[7]= 0xE000 | 1191 ; // HALT */ /* * Set up oscillator phase lock loop (PLL) for 24MHz bus speed. * (Procedure taken from HCS12 serial monitor code). */ CLKSEL_PLLSEL = 0; // Disengage PLL PLLCTL_PLLON = 1; // Turn on PLL //RCM 080915 for CSMB12 module with 4MHz instead of 8MHz on C32, set mult to 6 and div to 1 //SYNR = 0x02; // Set loop multiplier to 3 (SYNR = 2, mult=SYNR + 1) //RCM 070703 comm REFDV = 0x00; // Set input osc divider to 1 (REFDV = 0, div = REFDV+1) //REFDV = 0x01; SYNR = 0x05; // Set loop multiplier to 6 (SYNR = 5, mult=SYNR + 1) REFDV = 0x00; // set input osc divider to 1 (REFDV = 0, div =REFDV+1) _asm("nop"); _asm("nop"); // Delay for stabilization while( CRGFLG_LOCK != 1){}; // Wait until clock circuit locks on CLKSEL_PLLSEL = 1; // Engage PLL and go! /* * Set up initial port configurations. */ DDRAD=0xC0; /* set AN7 and AN6 to be outputs (LED4 and LED5) */ PERAD_PERAD0 = 1; /* engage pull-up on sensor pins (0 left and 1 right); makes OK if bumper switches not connected */ PERAD_PERAD1 = 1; //pushbuttons on CSMB12 module DDRP_DDRP0 = 0; //Enable as Input (PB1) DDRP_DDRP1 = 0; //Enable as Input (PB2) PERP_PERP0 = 1; //Enable Internal Pull-ups PERP_PERP1 = 1; //Enable Internal Pull-ups ATDDIEN = 0x0F; /* set AN0-AN3 as inputs (bumpers) */ /* set digit select bits of port M to be outputs * */ DDRM=0x3C; /* bits 5:4:3 of M register go to decoder, bit 2 is decimal point */ PTM=0xFF; /* */ /* set segment select bits of port T to be outputs */ DDRT=0xF1; /* upper 4 bits of T register go to 7-seg decoder */ PTT=0xFF; LEFT_LED_ON(FALSE); RIGHT_LED_ON(FALSE); /* * TSCR1_TEN: bit to enable entire timer system * TIOS: one bit per channel (bit=1 enables output capture) * TFLG1: one bit per channel (write 1 to bit to reset flag) * TCn: 16 bit register to hold compare value * TIE_CnI: bit enables channel interrupt (bit=1 enables) * TCTL2: pairs of bits set mode for external pin: * OM3:OL3 OM2:OL2 OM1:OL1 OM0:OL0 11=set; 10=clear; 00=no action; 01=toggle */ /* setup for output compare on T 0 */ TIOS=TIOS|0x01; //make channel 0 an output compare TC0 = 0x100; // load the compare count TCTL2= 0x03; // 0x03=set; 0x02=clear; 0x00=no action; 0x01 toggle TSCR1_TEN = 1; //enable timer system // TIE_C0I = 1; //enable interrupt on timer chan 0 TFLG1 = 0x01; //clear timer ch0 flag /* setup for output compare on T 1 */ TIOS= TIOS | 0x02; //make channel 1 an output compare TC1 = 0x100; // load the compare count TCTL2= TCTL2 | 0xC0; // 11=set; 10=clear; 00=no action; 01=toggle // TIE_C1I = 1; //enable interrupt on timer chan 1 TFLG1 = 0x02; //clear timer ch1 flag Speed_right= RIGHT_FORWARD; Speed_left = LEFT_FORWARD; MOTOR_INTERRUPTS_ON(FALSE); /* configure A/D converter, AN4 */ ATDCTL2 |= 0x80; ATDCTL2 &= (~0x62); ATDCTL4 &= (~0x61); /* * Start serial port */ initsci(); /* * Real Time Interrupt control setup */ //RCM 070703 comm RTICTL = 0x17; /* 0x27? 1ms rti period */ //was RCM 080915 RTICTL = 0x27; RTICTL = 0x13; //approx 976 microsec with 4MHz clock CRGFLG_RTIF = 1; // Set RTI flag to reset counter CRGINT_RTIE = 1; // Enable RTI interrupt EnableInterrupts; /* * Read DIP switches and dispatch */ pat=read_switches(); pat = (pat>>4)^0x0f; /* * Display revision code, for 1.5 seconds */ for(i=0;i<1500/(2*DELAY);i++){ display(1,CODE_REV_MAJOR,1); delay_ms(DELAY); display(0,CODE_REV_MINOR,0); delay_ms(DELAY); } /* * Display program number (DIP switch pattern), for 1.5 seconds */ for(i=0;i<1500/(4*DELAY);i++){ display_decimal(pat,-1); } display_off(); switch(pat){ case 0: default: board_test(); break; case 1: AtoD(); break; case 2: five_seconds_forward(); break; case 3: forward_backward(); break; case 4: dance(); break; case 5: bumper_bot(); break; case 6: prog_bot(); break; case 7: // RCM figure_eight(); break; }/* end switch */ for(;;) {} /* wait forever (should not reach this trap line) */ }/* end of main() */ /**********************************************************************************************/ /* * End of file * */ /**********************************************************************************************/