/* Program walk3.c File: walk3.c Author: Kerwin Lumpkins, Nov 2001 This is version 3 of the walker code. Previous versions got the thing walking, then added the lookdown sensor. This version improves on 1 and 2 by speeding the walking cycle up a bit and fine tuned the sensor routine. Version 3 added buzzer routine but they're commented out for now. Still need to iron this out so that motor routines aren't messed up. Description: This program provides control for a 6 legged walking robot that uses 3 servo motors (not modified, so they rotate through about 180 max range still) to control its motion. Servos Left and Right move the legs on that side of the bot forward or backward. Servo Center is mounted so that it "tilts" the robot to the left or right. While the bot it tilted, the legs on that side are free to swing through an arc forward or backward. The position to which the motor rotates is defined in this program as being either forward or backward (each is about 40 degrees off a a reference center postition) or centered. A servo rotates to a certain arc in response to a PWM (pulse width modulated) signal of about 40 Hz and with a duty cycle that varies depending on the position desired. The rough waveform diagram below gives def of these terms. 5 volts ------- ------- | | | | | | | | | | | | 0 volts ---- ------------------------------- ----- < duty c> < - - - - - - period - - - - - - - - - > duty c is duty cycle, meaning how long the pulse is "on duty" or ON. The period will be about 40 Hz so that the pulse is repeated about 40 times every second. For most servos the duty cycle will be about 1 - 3 ms. If you send it a 1 ms pulse, the servo will rotate Counter Clockwise as far as it can. If you send a 3 ms pulse, you get a Clockwise rotation. If you send it a value in the middle, it will rotate to the center or ref position. If it's already there, it won't move at all of course. 5 volts, 6 volts, whatever the servo runs on of course. The program simply sends a pulse to each motor in sequence to get the center motor to tilt the robot to the right, move the right legs forward, drop the center motor to ref position, center tilt to left, move left forward, center to ref position and then both right and left legs are centered, which pulls the robot forward. Turning and backwards motion are variations on this. The duty cycle time is determined by experimentation to get the best rotation time siganl. Then I use an internal clock time of 2.4576 MHz (9.8304 MHz external oscillator divided by 4 as spec'd by the HC08) to figure out how many clock cycles I need to count to get that time. Example: If I want to send a pulse of 1.75 ms. 2.4576 MHz = 407 ns / clock cycle (1/frequency). So 10^6 ns 1 clock cycle 1.75 ms * ------- * ------------- = 4301 clock cycles 1 ms 407 ns 4301 in decimal is 10CC in hex, so I need to set the counter that sets the duty cycle to count to 10CC hex (T1CH0H = 0x10 and T1CH0L = CC). Similarly, the modulus counter counts off the 40 Hz period. I set that hex C000. All I have to do then is define constants for all the positions that I want to rotate to (and they're different for each motor because the right motor rotates CW for forward, and the left does CCW). Then I call a function that sets the counter value up for the desired duty cycle and pass it that constant value. I simplified it even more by defining a function for each state to make the code more readable and then I wouldn't get lost and have to look up something on a table. So now there is a "move_left_forward" function, etc. This makes the code pretty self explanatory. ************* Lookdown sensor **************************** The lookdown sensor does an A/D convert on input from the signal. This is sampled after each time the bot pulls itself forward. If the value is at or below a threshold value then a dropoff condition exists and the bot drops into back up and turn right mode. ************* buzzer ************************************* The buzzer adds some "life" to the bot. It kind of looks like a bug so I added a buzzer to have it make some low frequency buzzing and clicking noises like a bug. It works okay but hurts the max speed of the bot and sounds to regular. I need to add a random function in for this so that the buzz comes at different times if at all ******************end description ***************************** */ #include /* library for the HC08GP32 */ #include /* standard library stuff for C */ #include /* These defines are the motor constants that define what position to rotate to */ #define lcntrhi 0x06 /* constants for the center ref position */ #define lcntrlo 0x80 #define rcntrhi 0x10 #define rcntrlo 0xCC #define ccntrhi 0x0B #define ccntrlo 0xD0 #define lfwdhi 0x02 /* constants for the forward positions */ #define lfwdlo 0x50 #define rfwdhi 0x14 #define rfwdlo 0x8E #define rfwdqtrhi 0x12 /* forward, 1/4 turn */ #define rfwdqtrlo 0xAD #define lfwdqtrhi 0x04 #define lfwdqtrlo 0x68 #define lbackhi 0x0A; /* backward positions */ #define lbacklo 0xB0; #define rbackhi 0x0D; #define rbacklo 0x0A; #define ctiltrthi 0x09 /* center motor tilt positions */ #define ctiltrtlo 0x50 #define ctiltlfthi 0x0E #define ctiltlftlo 0x00 #define dropoff_value 0x00A0 /* A/D trigger value for when dropoff exists */ // ****************** function prototypes ************** void move_left_forward (void); void move_right_forward (void); void move_left_center (void); void move_right_center (void); void move_center_center (void); void move_center_tiltright (void); void move_center_tiltleft (void); void move_left_backward (void); void move_right_backward (void); void move_left_forward_qtr (void); void move_right_forward_qtr (void); void delay (unsigned short time); int dropoff (void); void buzzer (int frequency); void walk (void); void backup (void); void turn_left (void); // ******************* MAIN ROUTINE ************************* void main () { int notdone = 1; // a while loop variable, always 1 for now // but it could be used to stop the bot unsigned short i; // loop variable int talkvar = 0; // for buzzer, not used in version 3 unsigned short talk = 0;// same // **************** timer channel 2 for Right motor ********************** T1SC = 0x20; //stop TIM1 counter T1SC = 0x30; //keep stopped, and reset counter T1MODH = 0xC0; //modulus high byte, for about 40 Hz period T1MODL = 0x00; //modulus low set T1SC0 = 0x1A; //unbuf output,toggle on overflow,set out on compare // **************** timer channel 2 for CENTER motor and buzzer ********************** T2SC = 0x20; //stop TIM2 counter T2SC = 0x30; //keep stopped, and reset counter T2MODH = 0xC0; //modulus high of T2MODL = 0x00; //modulus low set T2SC0 = 0x1A; //unbuf output,toggle on overflow,set out on compare T2SC1 = 0X00; // **************** timer channel 1 for Left motor ********************** T1SC1 = 0x1A; //unbuf output,toggle on overflow,set out on compare // SCI already turned on in _HC08Setup // called by the startup code SCBR = 0x02; // 9600 baud with a 9.8304MHz oscillator // Init the Analog to Digital Converter ADSCR = 0x3f; ADCLK = 0x30; printf("\r\r\rWelcome to Walker (not Texas Ranger) 3\r"); while ( notdone ) { if (dropoff() ){ // if loop for what to do if a dropoff detected backup (); turn_left(); } else { // if no dropoff, keep moving forward walk (); /* buzzer stuff talkvar = rand(); talk = talkvar &0x01; // mask off everything but LSB, see if odd or even talkvar = talkvar & 0xFF; if ( talk ) buzzer (talkvar); */ } // end else } // end while notdone } // end main // // END OF MAIN END OF MAIN END OF MAIN // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ // ********************************************************* // // function: walk - This function calls the necessary functions // to do one walk cycle // inputs: none // outputs: none void walk () { int talkvar = 0; unsigned short talk; T1SC = 0x20; // stop left and right motors, conserve power move_center_center(); delay (0xAFF); printf ("starting walk cycle\r"); move_left_center(); move_right_center(); delay (0x3FF8); T1SC = 0x20; // stop left and right motors move_center_tiltleft(); delay(0x1FF8); move_left_forward(); delay (0x3FF8); move_center_center(); delay (0xAFF); T1SC = 0x20; move_center_tiltright(); delay(0x1FF8); move_right_forward(); delay (0x3FF8); move_center_center(); delay (0xAFF); } // end walk // // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ // ************************************************************* // function: backup Calls functions necessary to back up the // the walkerbot. Runs when a dropoff is encountered // inputs: none // outputs: none void backup () { unsigned short i = 0; int talkvar = 0; unsigned short talk = 0; printf ("starting backup cycle\r"); for ( i=0; i < 3 ;i++ ) { // do this cycle 3 times move_center_center(); delay (0x3EF8); move_center_tiltleft(); delay(0x5EF8); move_left_backward(); delay (0x3EF8); move_center_center(); delay (0x3EF8); move_center_tiltright(); delay(0x5FF8); move_right_backward(); delay (0x3EF8); move_center_center(); delay (0x3EF8); move_left_center(); move_right_center(); delay (0x5EF8); move_left_forward(); move_right_forward(); delay (0x3EF8); } // end for loop printf ("Done with backup\r"); } // end backup // // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ // **************************************** // function: turn_left - does right turn using // both back left and forward right void turn_left () { unsigned short i = 0; int talkvar = 0; unsigned short talk = 0; printf ("starting turn left cycle\r"); for ( i=0; i < 8 ;i++ ) { // do this cycle 5 times move_left_center(); move_right_center(); delay (0x3FF8); move_center_center(); delay (0xAFF); move_center_tiltright(); delay (0xAFF); move_right_forward(); delay (0x3FF8); move_center_center (); delay (0xAFF); move_center_tiltleft(); delay (0xAFF); move_right_center(); delay (0x2FF8); T1SC = 0x20; // done } // end for loop printf ("Done with turn left\r"); } // end turn_left // // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ // ************************************************************* // function: dropoff - checks A/D channel 0 and compares to the // preset "dropoff" value, determined by experiment, constant set value // inputs: none // outputs: none int dropoff () { int sampled_value = 0; ADSCR = 00; while (!(ADSCR & 0x80)) { printf ("waiting"); // wait till COCO bit is set } // (conversion complete) sampled_value = ADR; printf ("\rsampled value is %x\r",sampled_value); if (sampled_value > dropoff_value)return 1; else return 0; } // end dropoff function // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ // ********************************************************* // // functions: move - These are drivers for the servo motors // it sets up the PWM generators on PortD // inputs: direction and time to run the sequence // outputs: none, this is a low level driver void move_left_center () { int i = 0; T1CH0H = lcntrhi; T1CH0L = lcntrlo; T1SC = 0x10; // PWM generator ON } // end move right center function // // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ void move_left_forward () { int i = 0; T1CH0H = lfwdhi; T1CH0L = lfwdlo; T1SC = 0x10; // PWM generator ON } // end move right center function // void move_left_forward_qtr () { int i = 0; T1CH0H = lfwdqtrhi; T1CH0L = lfwdqtrlo; T1SC = 0x10; // PWM generator ON } // end move right forward qtr function // // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ void move_left_backward () { int i = 0; T1CH0H = lbackhi; T1CH0L = lbacklo; T1SC = 0x10; // PWM generator ON } // end move right center function // void move_right_center () { int i = 0; T1CH1H = rcntrhi; T1CH1L = rcntrlo; T1SC = 0x10; // PWM generator ON } // end move right center function // // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ void move_right_forward () { int i = 0; T1CH1H = rfwdhi; T1CH1L = rfwdlo; T1SC = 0x10; // PWM generator ON } // end move right forward function // // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ void move_right_forward_qtr () { int i = 0; T1CH1H = rfwdqtrhi; T1CH1L = rfwdqtrlo; T1SC = 0x10; // PWM generator ON } // end move right forward qtr function // // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ void move_right_backward () { int i = 0; T1CH1H = rbackhi; T1CH1L = rbacklo; T1SC = 0x10; // PWM generator ON } // end move right backward function // // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ void move_center_center () { int i = 0; T2CH0H = ccntrhi; T2CH0L = ccntrlo; T2SC = 0x10; // PWM generator ON } // end move center center function // // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ void move_center_tiltright () { int i = 0; T2CH0H = ctiltrthi; T2CH0L = ctiltrtlo; T2SC = 0x10; // PWM generator ON } // end move center tilt right function // // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ void move_center_tiltleft () { int i = 0; T2CH0H = ctiltlfthi; T2CH0L = ctiltlftlo; T2SC = 0x10; // PWM generator ON } // end move center tilt right function // // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ void delay (unsigned short ftime) { unsigned short i = 0; for (i = 0; i<= (ftime -1);i++) { } } // end delay function // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ // ***************************************************** // function: buzzer - Takes an input between 1-255 (odd only) // and calculates a frequency by multiplying by 10, then // parses this value up into high and low bytes for frequency // and duty cycle // inputs: the talkvar value made from rand function // ouputs: none (from the function at least, the buzzer makes noise) void buzzer (int makefreq) { unsigned short i = 0; int wfreq = 0; int freqh = 0; int freql = 0; int dutyh = 0; int dutyl = 0; wfreq = makefreq ; freqh = ((wfreq & 0xFF00) >> 8); freql = wfreq & 0xFF; dutyh = freqh/2; dutyl = freql/2; printf ("in buz function. makefreq is %x, so wfreq is %x\r",makefreq,wfreq); printf ("freq values are %x, %x. Duty cycle is %x, %x\r", freqh,freql,dutyh,dutyl); T2SC = 0x20; //stop TIM2 counter T2SC = 0x30; //keep stopped, and reset counter T2MODH = freqh; T2MODL = freql; T2CH1H = dutyh; T2CH1L = dutyl; T2SC1 = 0x1A; //unbuf output,toggle on overflow,set out on compare T2SC = 0x10; // PWM generator ON for (i=0;i<=0x5800;i++) { // loop to keep buzzer on a while } // stop TIM2 counter to stop buzzer, then reset TIM2 for center motor T2SC = 0x20; //stop TIM2 counter T2SC = 0x30; //keep stopped, and reset counter T2MODH = 0xC0; //modulus high of T2MODL = 0x00; //modulus low set T2SC0 = 0x1A; //unbuf output,toggle on overflow,set out on compare T2SC1 = 0X00; } // // end of buzzer function // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^