/* Software for Ebot 7/10/01 Alex Brown rbirac@home.com saved as: E:\xxxx written in ImageCraft ICC12 C for the Adapt912 board (68HC912B32) Program sends robot straight out for 11 feet, pivots 180 degrees and returns. After a delay, it does a squared off figure 8 pattern. */ /* I/O usage pin Name Use Drive motor PWM command Left Drive motor PWM command Right 13 PT0 Drive wheel encoder input A Left 12 PT1 Drive wheel encoder input A Right 11 PT2 Drive wheel encoder input B Left 10 PT3 Drive wheel encoder input B Right */ #include <912b32.h> //Set the reset vector to jump to 8000 (unique to this controller) #pragma abs_address:0xF7FE const int startxx = 0x8000; //the name startxx is arbitrary #pragma end_abs_address //Procedure declarations void rtexec(void); //Global Variable definitions // CLOCK int time; // counts real time clock ticks (16.384 msec) // EXEC CONTROL int mode; // Case number for operating mode int modedone; // 1 indicates mode completed int step; // pointer to mode table int branch; // flag meaning take branch option of table int BR; // branch label number //Motor Control constants (pulse width values in microseconds) const int StopLeft = 1500; const int StopRight = 1500; const int FwdFastLeft = 1700; const int FwdFastRight = 1100; const int FwdSlowLeft = 1662; const int FwdSlowRight = 1250; const int RevSlowRight = 1600; //Encoders long int odomL; //raw count left encoder long int odomR; //raw count right encoder //Navigation int Vmax; // speed for motors (not used) int dist; // distance traveled in encoder clicks int distcmd; // distance to travel in encoder clicks int steercmd; // steering command in delta pwm msec. int KPsteer = 50; // steering proportional gain //Misc variables int i; //used for temp counters int pflg,p1,p2,p3,p4,p5; //print variables //Navigation table for Ebot 5/2/01 #define NavSize 50 const int nav[NavSize][7] = { //Label Mode MaxSpd FinSpd Dist Option Branch // do practice contest. Fwd 10 feet, pivot 180 & return 0, 0, 0, 0, 61, 0x00, 0, //wait 1 second 0, 1, 0, 0, 421, 0x00, 0, //fwd for 11 feet 0, 2, 0, 0, 52, 0x00, 0, //pivot right 180 0, 1, 0, 0, 430, 0x00, 0, //fwd for 11+ feet 0, 99, 0, 0, 610, 0x00, 0, //stops here // do a figure 8 & repeat 10, 1, 0, 0, 150, 0x00, 0, //fwd 1.5 feet 0, 2, 0, 0, 35, 0x00, 0, //pivot right 90 0, 1, 0, 0, 150, 0x00, 0, //fwd 1.5 feet 0, 2, 0, 0, 35, 0x00, 0, //pivot right 90 0, 1, 0, 0, 300, 0x00, 0, //fwd 3 feet 0, 3, 0, 0, 39, 0x00, 0, //pivot left 90 0, 1, 0, 0, 150, 0x00, 0, //fwd 1.5 feet 0, 3, 0, 0, 39, 0x00, 0, //pivot left 90 0, 1, 0, 0, 150, 0x00, 0, //fwd 1.5 feet 0, 3, 0, 0, 39, 0x00, 0, //pivot left 90 0, 1, 0, 0, 300, 0x00, 0, //fwd 3 feet 0, 2, 0, 0, 35, 0x00, 0, //pivot right 90 0, 90, 0, 0, 0, 0x00, 10, //branch to 10 0, 99, 0, 0, 0, 0x00, 0, //stop }; //--------------------------------------------------------------------------- //Real time clock interrupt #pragma interrupt_handler rti_handler() void rti_handler(void) { RTIFLG = 0x0080; //reset RTI flag time = time + 1; //increment time counter INTR_ON(); //allows encoder interrupts to continue rtexec(); //run real time executive } #pragma abs_address:0xF7F0 //jump table address for RTI interrupt void (*rti_isr[])() = {rti_handler}; #pragma end_abs_address //--------------------------------------------------------------------------- //Drive wheel encoder interrupt Left (TC0) #pragma interrupt_handler odom_handler_L() void odom_handler_L(void) { TFLG1 = 0x01; //Reset timer 0 interrupt flag if (PORTT & 0x04) odomL = odomL + 1; //increment raw encoder count else odomL = odomL - 1; } #pragma abs_address:0xF7EE //jump table address void (*odom_isrL[])() = {odom_handler_L}; #pragma end_abs_address //--------------------------------------------------------------------------- //Drive wheel encoder interrupt Right (TC1) #pragma interrupt_handler odom_handler_R() void odom_handler_R(void) { TFLG1 = 0x02; //Reset timer 1 interrupt flag if (PORTT & 0x08) odomR = odomR - 1; //increment raw encoder count else odomR = odomR + 1; } #pragma abs_address:0xF7EC //jump table address void (*odom_isrR[])() = {odom_handler_R}; #pragma end_abs_address /*--------------------------------------------------------------------------- Initialization of timer for servo pwm & encoders TC0 used for drive wheel encoder left capture (chan A) TC1 used for drive wheel encoder right capture (chan A) TC2 used for drive wheel encoder left (direction, chan B) TC3 used for drive wheel encoder right (direction, chan B) TC4 Left drive servo TC5 Right drive servo */ void timerinit() { TMSK1 = 0x03; //enable timer interrupts TC0 & TC1 TMSK2 = 0x2B; //disable overflow intr, enable pull up, //prescale = 8, reset after OC7 compare TCTL1 = 0x0A; //init OC4 and OC5 to clear on compare TCTL4 = 0x05; //TC0 & TC1 to cap on rising edge TIOS = 0xB0; //Select chan 4,5 & 7 as output compare OC7M = 0x30; //Init (C7 compare to affect output 4 & 5 OC7D = 0x30; //enable OC7 to set OC4 & OC5 high TC7 = 0x4E20; //load compare period for 20 msec. TC4 = 1500; //init TC4 to 1.5 millisec (approx stopped) TC5 = 1500; //init TC5 to 1.5 millisec (approx stopped) TSCR = 0x80; //enable timer } //--------------------------------------------------------------------------- /* Real Time executive routine for Ebot. Runs each time RTI interrupt occurs and calls appropriate tasks. If current task is completed (modedone = 1), it sets up next task. */ void rtexec() {if (modedone == 1) //if previous mode done, set up next mode {if (branch) //if branch set, find label step {for (i = 0; iNavSize-1) printf(" branch not found \n"); }; }; branch = 0; mode = nav[step][1]; //set up next mode distcmd = nav[step][4]; //distance to travel(in seconds/61) BR = nav[step][6]; //branch label time = 0; //reset time counter for new mode modedone = 0; //set mode to not done step = step + 1; //default next step }; switch (mode) //select the case for the current mode {case 0: //stop and wait the time in dist TC4 = StopLeft; //stop left TC5 = StopRight; //stop right if (time >distcmd) modedone = 1; //wait dist/61 second then start break; case 1: //go straight for the time in dist if (time == 0) { odomL = 0; odomR = 0; }; dist = (odomL + odomR)/2; steercmd = (odomL - odomR)* KPsteer; //steercmd = 0; TC4 = FwdFastLeft - steercmd/2; TC5 = FwdFastRight + steercmd/2; if (pflg==0) {p1 = odomL;p2=odomR;p3=steercmd;p4=TC4;p5=TC5;pflg = 1;}; if (dist >distcmd) modedone = 1; break; case 2: //pivot to right TC4 = FwdSlowLeft; //fwd slow left TC5 = RevSlowRight; //rev slow right if ((odomL-odomR) > distcmd) modedone = 1; break; case 3: //pivot to left TC4 = FwdSlowLeft; TC5 = 1250; //fwd slow right if (time >dist) modedone = 1; break; case 90: //Branch branch = 1; modedone = 1; break; case 99: //Stop & done TC4 = StopLeft; //stop left TC5 = StopRight; //stop right break; } } main() { //set up serial communications setbaud(BAUD9600); //INITIALIZATIONS FEELCK = 0x01; //disable ability to write to boot flash timerinit(); //initialize pwm //SET UP interrupts RTICTL = 0x85; //enable RTI at 16.384 msec. RTIFLG = 0x80; //clear real time interrupt flag //--------------------------------------------------------------------------- // Control program printf(" Ebot 5/2/01\n"); modedone = 1; //initialize as mode done so exec will go to next INTR_ON(); //turn on Real Time Interrupt while (1) //do anything desired in background. All control in in rtexec() { if (pflg==1) {printf(" %5d %5d %5d %5d %5d \n",p1,p2,p3,p4,p5); pflg = 0; }; }; return(0); }